mavproxy_SIYI: add option to stow the camera when vehicle starts to land

Author: peterbarker

Diff for: 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: