From fcb954d4e8fb791f0ad36739916c27296b8212c3 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 12 Apr 2024 16:00:32 +1000 Subject: [PATCH] mavproxy_SIYI: add option to stow the camera when vehicle starts to land --- MAVProxy/modules/mavproxy_SIYI/__init__.py | 40 ++++++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/MAVProxy/modules/mavproxy_SIYI/__init__.py b/MAVProxy/modules/mavproxy_SIYI/__init__.py index fdb07470b9..6104f963e7 100644 --- a/MAVProxy/modules/mavproxy_SIYI/__init__.py +++ b/MAVProxy/modules/mavproxy_SIYI/__init__.py @@ -295,6 +295,7 @@ def __init__(self, mpstate): MPSetting('thresh_climit_dis', int, 20, range=(10,50)), MPSetting('thresh_volt_dis', int, 40, range=(20,80)), MPSetting('thresh_ang_dis', int, 40, range=(30,4000)), + ('stow_on_landing', bool, True), ]) self.add_completion_function('(SIYISETTING)', self.siyi_settings.completion) @@ -366,6 +367,12 @@ def __init__(self, mpstate): self.last_getconfig = time.time() self.click_mode = "Flag" + # support for stowing the camera when we start to land: + self.extended_sys_state_received_time = 0 + self.extended_sys_state_request_time = 0 + self.extended_sys_state_warn_time = 0 # last time we warned about not being able to auto-stow + self.last_landed_state = None + if mp_util.has_wxpython: menu = MPMenuSubMenu('SIYI', items=[ @@ -1470,6 +1477,14 @@ def mavlink_packet(self, m): self.show_fov() except Exception as ex: print(traceback.format_exc()) + elif mtype == 'EXTENDED_SYS_STATE': + if self.settings.stow_on_landing: + self.extended_sys_state_received_time = time.time() + if (m.landed_state == mavutil.mavlink.MAV_LANDED_STATE_LANDING and + self.last_landed_state != mavutil.mavlink.MAV_LANDED_STATE_LANDING): + # we've just transition into landing + self.cmd_angle([0, 0]) + self.last_landed_state = m.landed_state def receive_thread(self): '''thread for receiving UDP packets from SIYI''' @@ -1510,6 +1525,31 @@ def idle_task(self): self.check_thermal_events() self.therm_capture() + # we need EXTENDED_SYS_STATE to work out if we're landing: + if self.settings.stow_on_landing: + now = time.time() + delta_t = now - self.extended_sys_state_received_time + delta_sent_t = now = self.extended_sys_state_request_time + if delta_t > 5 and delta_sent_t > 5: + self.extended_sys_state_request_time = now + self.master.mav.command_long_send( + 1, # FIXME, target_system + 1, # FIXME, target_component + mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL, + 0, # confirmation + mavutil.mavlink.MAVLINK_MSG_ID_EXTENDED_SYS_STATE, # p1 + 1e6, #p2 interval (microseconds) + 0, #p3 instance + 0, + 0, + 0, + 0 + ) + delta_warn_t = now - self.extended_sys_state_warn_time + if delta_warn_t > 60: + self.extended_sys_state_warn_time = now + print("SIYI: no EXTENDED_SYS_STATE, can't auto-stow") + def show_horizon_lines(self): '''show horizon lines''' if self.rgb_view is None or self.rgb_view.im is None: