Antenna encoder fix

src/Bringup/launch/antenna.launch.py

@@ -29,8 +29,8 @@ def generate_launch_description():
                     },
                     {"Baudrate": 115200},
                     {"Freq": 5.0},  # Publish rate (hz)
-                    {"SvinMindur": 300},  # Survey in time (s)
-                    {"SvinMinAccDur": 10_000},  # Survey in accuracy (mm)
+                    {"SvinMinDur": 300},  # Survey-in minimum duration (s)
+                    {"SvinAccLimit": 10_000},  # Survey-in accuracy limit (mm)
                     {"QueueDepth": 10},
                 ],
             ),
@@ -61,7 +61,10 @@ def generate_launch_description():
                         "CountsPerRev": 8192  # based on 4096 encoder resolution and 2:1 gear ratio
                     },
                 ],
-                remappings=[("/roboclaw_position", "/antenna/tracker_bearing")],
+                remappings=[
+                    ("/roboclaw_desired_position", "/antenna/target_bearing"),
+                    ("/roboclaw_actual_position", "/antenna/bearing"),
+                ],
             ),
             # Optionally run the standalone basestation config script when requested
             ExecuteProcess(

src/HW-Devices/ros_roboclaw/ros_roboclaw/antenna_roboclaw_node.py

@@ -50,18 +50,23 @@ def __init__(self):
         self.zero_offset = 0
 
         # ROS Interfaces
-        self.create_subscription(Float32, "/roboclaw_position", self.pos_callback, 5)
+        self.create_subscription(
+            Float32, "/roboclaw_desired_position", self.pos_callback, 5
+        )
         self.create_timer(1.0 / self.enc_read_freq, self.encoder_timer)
+        self.antenna_encoder_pub = self.create_publisher(
+            Float32, "/roboclaw_actual_position", 10
+        )
 
     # Angle to Encoder Command
     def pos_callback(self, msg: Float32):
-        norm = msg.data
+        norm = (float(msg.data) % (math.pi * 2)) / (math.pi * 2)
 
         self.target_encoder = self.zero_offset + int(norm * self.counts_per_rev)
 
         error = self.wrap_error(self.target_encoder - self.current_encoder)
 
-        self.get_logger().info(f"target= {norm * 360:.2f}°, {self.target_encoder}")
+        # self.get_logger().info(f"target= {norm * 360:.2f}°, {self.target_encoder}")
 
         self.drive_to_position(self.current_encoder + error)
 
@@ -82,6 +87,10 @@ def encoder_timer(self):
             return
 
         self.current_encoder = enc
+        enc_rel = (enc - self.zero_offset) % self.counts_per_rev
+        antenna_radians = Float32()
+        antenna_radians.data = (float(enc_rel) / self.counts_per_rev) * (math.pi * 2)
+        self.antenna_encoder_pub.publish(antenna_radians)
 
     # Position Control
     def drive_to_position(self, target):

src/Nav/gps/gps/antenna_pointing_node.py

@@ -33,9 +33,7 @@ def __init__(self):
         self.create_subscription(NavSatFix, "/base_station/fix", self.base_cb, 2)
         self.create_subscription(NavSatFix, "/gps/fix", self.rover_cb, 2)
 
-        self.bearing_pub = self.create_publisher(
-            Float32, "/antenna/tracker_bearing", 10
-        )
+        self.bearing_pub = self.create_publisher(Float32, "/antenna/target_bearing", 10)
 
         self.timer = self.create_timer(1.0 / self.freq, self.update)
 
@@ -76,9 +74,7 @@ def update(self):
 
     def publish_bearing(self, bearing):
         msg = Float32()
-        msg.data = float(bearing) / (
-            math.pi * 2
-        )  # normalize the output for the roboclaw
+        msg.data = float(bearing)  # radians follow the ros standard, don't normalize
         self.bearing_pub.publish(msg)
 
     def bearing(self, lat1, lon1, lat2, lon2):