camtrack: camera and gimbal component ids should match autopilot

- camera_view: add variable for tracking image status update rate
- camera_view: camera and gimbal component ids should match autopilot

Signed-off-by: Rhys Mainwaring <[email protected]>

Author: srmainwaring

MAVProxy/modules/mavproxy_camtrack/__init__.py

@@ -43,7 +43,7 @@ def __init__(self, mpstate):
 
         self.mpstate = mpstate
 
-        # GUI
+        # Settings
         # TODO: provide args to set RTSP server location
         # localhost simulation
         rtsp_url = "rtsp://127.0.0.1:8554/camera"
@@ -57,6 +57,9 @@ 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
+
+        # GUI
         self.camera_view = CameraView(self.mpstate, "Camera Tracking", rtsp_url)
 
         # NOTE: unused except for status
@@ -76,12 +79,6 @@ def __init__(self, mpstate):
         self._do_request_camera_information = True
         self._do_request_camera_tracking_image_status = True
 
-        # control update rate to GUI
-        self._msg_list = []
-        self._fps = 30.0
-        self._last_send = 0.0
-        self._send_delay = (1.0 / self._fps) * 0.9
-
         # commands
         self.add_command("camtrack", self.cmd_camtrack, "camera tracking")
 
@@ -155,7 +152,7 @@ def mavlink_packet(self, msg):
         elif mtype == "CAMERA_TRACKING_IMAGE_STATUS":
             self.handle_camera_tracking_image_status(msg)
 
-        # TODO: NOTE: disabled
+        # TODO: NOTE: ack checks are disabled
         # check command_ack
         elif False:  # mtype == "COMMAND_ACK":
             if msg.command == mavutil.mavlink.MAV_CMD_CAMERA_TRACK_POINT:
@@ -215,7 +212,7 @@ def mavlink_packet(self, msg):
             elif msg.command == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL:
                 print("Got COMMAND_ACK: MAV_CMD_SET_MESSAGE_INTERVAL")
 
-        # TODO: NOTE: disabled
+        # TODO: NOTE: command processing disabled
         # check command_long
         elif False:  # mtype == "COMMAND_LONG":
             # TODO: check target_system is for offboard control
@@ -293,15 +290,7 @@ def check_events(self):
         #     self.needs_unloading = True
 
     def send_messages(self):
-        """Send message list via pipe to GUI at desired update rate"""
-        if (time.time() - self._last_send) > self._send_delay:
-            # pipe data to GUI
-            # TODO: check interface in view for pipe updates
-            # self.camera_view.parent_pipe_send.send(self._msg_list)
-            # reset counters etc
-            self._msg_list = []
-            self._last_send = time.time()
-
+        """Send messages"""
         # TODO: implement camera and gimbal discovery correctly
         # Discovery - most of these requests are handled in the FC
         #             by GCS_MAVLINK::try_send_message
@@ -335,9 +324,10 @@ def send_messages(self):
             self._do_request_camera_information = False
 
         if self._do_request_camera_tracking_image_status:
+            interval_us = int(1e6 / self._camera_tracking_image_status_rate)
             self.send_set_message_interval_message(
                 mavutil.mavlink.MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS,
-                1000 * 50,  # 20Hz
+                interval_us,  # requested interval in microseconds
                 response_target=1,  # flight-stack default
             )
             self._do_request_camera_tracking_image_status = False
@@ -402,12 +392,9 @@ def idle_task(self):
 
     def unload(self):
         """Close the GUI and unload module"""
-
-        # close the GUI
         self.camera_view.close()
 
 
 def init(mpstate):
     """Initialise module"""
-
     return CamTrackModule(mpstate)

MAVProxy/modules/mavproxy_camtrack/camera_view.py

@@ -32,16 +32,15 @@ def __init__(self, mpstate, title, rtsp_url, fps=30):
 
         self.frame_counter = -1
 
-        # TODO: gimbal and camera system ids
+        # Commands to autopilot attached cameras and gimbals are addressed
+        # to the autopilot component.
+        self.camera_sysid = 1  # system id matches vehicle
+        self.camera_cmpid = 1  # component id matches autopilot
+        self.camera_deviceid = 1  # first flight-stack connected camera
 
-        # autopilot component may proxy up to 6 cameras
-        self.camera_sysid = 1
-        self.camera_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER
-        self.camera_deviceid = 1  # first autopilot attached camera
-
-        self.gimbal_sysid = 1
-        self.gimbal_cmpid = mavutil.mavlink.MAV_COMP_ID_ONBOARD_COMPUTER
-        self.gimbal_deviceid = 1  # first autopilot attached gimbal
+        self.gimbal_sysid = 1  # system id matches vehicle
+        self.gimbal_cmpid = 1  # component id matches autopilot
+        self.gimbal_deviceid = 1  # first flight-stack connected gimbal
 
         self.im = TrackerImage(
             title=title,
@@ -75,13 +74,6 @@ def set_tracked_rectangle(self, top_left_x, top_left_y, bot_right_x, bot_right_y
 
     def close(self):
         """Close the GUI"""
-        # TODO: MPImage does not have a close_event
-        # trigger a close event which is monitored by the
-        # child gui process - it will close allowing the
-        # process to be joined
-        # self.im.close_event.set()
-        # if self.im.is_alive():
-        #     self.im.child.join(timeout=2.0)
         self.im.terminate()
 
     def is_alive(self):