diff --git a/MAVProxy/modules/mavproxy_camtrack/__init__.py b/MAVProxy/modules/mavproxy_camtrack/__init__.py index 3333bc488f..264a71e624 100644 --- a/MAVProxy/modules/mavproxy_camtrack/__init__.py +++ b/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) diff --git a/MAVProxy/modules/mavproxy_camtrack/camera_view.py b/MAVProxy/modules/mavproxy_camtrack/camera_view.py index fef377c977..802e780471 100644 --- a/MAVProxy/modules/mavproxy_camtrack/camera_view.py +++ b/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):