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

MAVProxy/modules/mavproxy_SIYI/__init__.py

@@ -295,6 +295,10 @@ 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),
+                                                     ('stow_heuristics_enabled', bool, True),
+                                                     ('stow_heuristics_minalt', float, 20.0),  # metres above terrain
+                                                     ('stow_heuristics_maxhorvel', float, 2.0),  # metres/second
                                                          ])
         self.add_completion_function('(SIYISETTING)',
                                      self.siyi_settings.completion)
@@ -366,6 +370,19 @@ 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
+
+        # support retracting camera based on heuristics:
+        self.landing_heuristics = {
+            "armed": True,
+            "last_warning_ms": 0,
+            "current_terrain_alt": 0,
+        }
+
         if mp_util.has_wxpython:
             menu = MPMenuSubMenu('SIYI',
                                  items=[
@@ -1470,6 +1487,18 @@ def mavlink_packet(self, m):
                 self.show_fov()
             except Exception as ex:
                 print(traceback.format_exc())
+        elif mtype == 'EXTENDED_SYS_STATE':
+            if self.siyi_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.retract()
+                self.last_landed_state = m.landed_state
+
+    def retract(self):
+        print("SIYI: stowing camera")
+        self.cmd_angle([0, 0])
 
     def receive_thread(self):
         '''thread for receiving UDP packets from SIYI'''
@@ -1510,6 +1539,73 @@ 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.siyi_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
+                print("requiesting")
+                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_t > 60 and delta_warn_t > 60:
+                self.extended_sys_state_warn_time = now
+                print("SIYI: no EXTENDED_SYS_STATE, can't auto-stow")
+
+        if self.siyi_settings.stow_heuristics_enabled:
+            self.check_stow_on_landing_heuristics()
+
+    def check_stow_on_landing_heuristics(self):
+        tr = self.master.messages.get('TERRAIN_REPORT', None)
+        vfr_hud = self.master.messages.get('VFR_HUD', None)
+
+        if tr is None or vfr_hud is None:
+            now = time.time()
+            if now - self.landing_heuristics["last_warning_ms"] > 60:
+                self.landing_heuristics["last_warning_ms"] = now
+                print("SIYI: missing messages, hueristics-stow not available")
+            return
+
+        # first work out whether we should "arm" the stowing; must
+        # meet minimum conditions to do so:
+        current_terrain_alt = tr.current_height
+        if current_terrain_alt > self.siyi_settings.stow_heuristics_minalt + 1:
+            # above trigger altitude
+            self.landing_heuristics["armed"] = True
+
+        # now work out whether to stow:
+        if not self.landing_heuristics["armed"]:
+            return
+
+        if current_terrain_alt > self.siyi_settings.stow_heuristics_minalt:
+            # above the minimum altitude
+            return
+
+        if vfr_hud.groundspeed > self.siyi_settings.stow_heuristics_maxhorvel:
+            # travelling rapidly horizontally
+            return
+
+        if vfr_hud.climb > 0:
+            # climbing
+            return
+
+
+        self.landing_heuristics["armed"] = False
+        self.retract()
+
     def show_horizon_lines(self):
         '''show horizon lines'''
         if self.rgb_view is None or self.rgb_view.im is None: