camtrack: intial tune of status rates and pid gains

- Reduce image status rate to 20 Hz
- Set response_target in set message interval to flight-stack default
- Add debug info for image status / tracker_pos
- onboard_controller: apply formatter
- onboard_controller: adjust controller gains

Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com>

Author: srmainwaring

MAVProxy/modules/mavproxy_camtrack/__init__.py

@@ -57,7 +57,7 @@ def __init__(self, mpstate):
         # SIYI A8 camera
         # rtsp_url = "rtsp://192.168.144.25:8554/main.264"
 
-        self._camera_tracking_image_status_rate = 50  # Hz
+        self._camera_tracking_image_status_rate = 20  # Hz
 
         # GUI
         self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url)
@@ -328,7 +328,7 @@ def send_messages(self):
             self.send_set_message_interval_message(
                 mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
                 interval_us,  # requested interval in microseconds
-                response_target=1,  # flight-stack default
+                response_target=0,  # flight-stack default
             )
             self._do_request_camera_tracking_image_status = False
 
@@ -369,9 +369,9 @@ def send_request_message(self, message_id, p1=0):
         )
 
     def send_set_message_interval_message(
-        self, message_id, interval_us, response_target=1
+        self, message_id, interval_us, response_target=0
     ):
-        self.master.mav.command_long_send(
+        msg = self.master.mav.command_long_encode(
             self.settings.target_system,  # target_system
             self.settings.target_component,  # target_component
             mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL,  # command
@@ -384,6 +384,7 @@ def send_set_message_interval_message(
             0,  # param6
             response_target,  # param7
         )
+        self.master.mav.send(msg)
 
     def idle_task(self):
         """Idle tasks"""

MAVProxy/modules/mavproxy_camtrack/onboard_controller.py

@@ -697,6 +697,12 @@ def _send_camera_tracking_image_status(self):
             rec_bottom_x,
             rec_bottom_y,
         )
+        # NOTE: leave for debugging
+        # print(
+        #     f"image_status: x: {rec_top_x:.2f}, y: {rec_top_y:.2f}, "
+        #     f"w: {(rec_bottom_x - rec_top_x):.2f}, "
+        #     f"h: {(rec_bottom_y - rec_top_y):.2f}"
+        # )
         self._connection.mav.send(msg)
 
     def _handle_camera_track_point(self, msg):
@@ -806,7 +812,7 @@ class GimbalController:
 
     def __init__(self, connection):
         self._connection = connection
-        self._sysid = self._connection.source_system # system id matches vehicle
+        self._sysid = self._connection.source_system  # system id matches vehicle
         self._cmpid = 1  # component id matches autopilot
 
         print(f"Gimbal Controller: src_sys: {self._sysid}, src_cmp: {self._cmpid})")
@@ -831,8 +837,8 @@ def __init__(self, connection):
         self._gimbal_failure_flags = None
 
         # TODO: add options for PI controller gains
-        self._pitch_controller = PI_controller(Pgain=0.1, Igain=0.01, IMAX=0.1)
-        self._yaw_controller = PI_controller(Pgain=0.1, Igain=0.1, IMAX=1.0)
+        self._pitch_controller = PI_controller(Pgain=0.1, Igain=0.02, IMAX=0.3)
+        self._yaw_controller = PI_controller(Pgain=0.1, Igain=0.02, IMAX=0.3)
 
         # Start the control thread
         self._control_in_queue = queue.Queue()
@@ -911,8 +917,8 @@ def _control_task(self):
                 # the current mount orientation.
                 _, ay, az = euler.quat2euler(gimbal_orientation)
 
-                self._pitch_controller.gain_mul = 4.0
-                self._yaw_controller.gain_mul = 40.0
+                self._pitch_controller.gain_mul = 5.0
+                self._yaw_controller.gain_mul = 5.0
 
                 err_pitch = self._neutral_y - ay
                 pitch_rate_rads = self._pitch_controller.run(err_pitch)
@@ -937,8 +943,8 @@ def _control_task(self):
                     err_x = act_x - tgt_x
                     err_y = -(act_y - tgt_y)
 
-                self._pitch_controller.gain_mul = 1.0
-                self._yaw_controller.gain_mul = 2.0
+                self._pitch_controller.gain_mul = 0.5
+                self._yaw_controller.gain_mul = 0.5
 
                 err_pitch = math.radians(err_y)
                 pitch_rate_rads = self._pitch_controller.run(err_pitch)
@@ -993,7 +999,7 @@ class PI_controller:
     MAVProxy/modules/mavproxy_SIYI/PI_controller (modified)
     """
 
-    def __init__(self, Pgain, Igain, IMAX, gain_mul=1.0, max_rate=math.radians(30.0)):
+    def __init__(self, Pgain, Igain, IMAX, gain_mul=1.0, max_rate=math.radians(90.0)):
         self.Pgain = Pgain
         self.Igain = Igain
         self.IMAX = IMAX

MAVProxy/modules/mavproxy_camtrack/tracker_image.py

@@ -279,6 +279,13 @@ def get_position(self):
 
     def set_position(self, tracker_pos):
         self._tracker_pos = tracker_pos
+        # NOTE: leave for debugging
+        # print(
+        #     f"tracker_pos: x: {self._tracker_pos.left():.2f}, "
+        #     f"y: {self._tracker_pos.top():.2f}, "
+        #     f"w: {(self._tracker_pos.right() - self._tracker_pos.left()):.2f}, "
+        #     f"h: {(self._tracker_pos.bottom() - self._tracker_pos.top()):.2f}"
+        # )
 
 
 def create_tracker(name, state):