Ap mode setting (#1852)

* [rotorcraft] allow to change some modes from settings even without RC

see #1819

* [rotorcraft] allow some more modes without RC

* [joystick] change mode directly instead of rc_auto2

* [rotorcraft] change handler name to avoid confusion

conf/settings/rotorcraft_basic.xml

@@ -10,7 +10,7 @@
         <strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
         <strip_button name="POWER OFF" icon="off.png" value="0" group="power_switch"/>
       </dl_setting>
-      <dl_setting var="autopilot_mode" min="0" step="1" max="2" module="autopilot" shortname="mode" values="KILL|Fail|HOME" handler="set_mode"/>
+      <dl_setting var="autopilot_mode" min="0" step="1" max="19" module="autopilot" shortname="mode" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip|Guided" handler="SetModeHandler"/>
     </dl_settings>
 
   </dl_settings>

conf/settings/rotorcraft_basic_multi.xml

@@ -10,7 +10,7 @@
         <strip_button name="POWER ON" icon="on.png" value="1" group="power_switch"/>
         <strip_button name="POWER OFF" icon="off.png" value="0" group="power_switch"/>
       </dl_setting>
-      <dl_setting var="autopilot_mode" min="0" step="1" max="2" module="autopilot" shortname="mode" values="KILL|Fail|HOME" handler="set_mode"/>
+      <dl_setting var="autopilot_mode" min="0" step="1" max="19" module="autopilot" shortname="mode" values="KILL|Fail|HOME|Rate|Att|Rate_rcC|Att_rcC|Att_C|Rate_Z|Att_Z|Hover|Hover_C|Hover_Z|Nav|RC_D|CareFree|Forward|Module|Flip|Guided" handler="SetModeHandler"/>
       <dl_setting var="multi_gps_mode" min="0" step="1" max="2" module="subsystems/gps" shortname="gpsmode" values="AUTO|PRIMARY|SECONDARY">
         <strip_button name="AUTO" icon="gps.png" value="0" group="gps_mode"/>
         <strip_button name="PRIMARY" icon="gps1.png" value="1" group="gps_mode_setting"/>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -401,6 +401,25 @@ void autopilot_periodic(void)
 
 }
 
+/** AP mode setting handler
+ *
+ * Checks RC status before calling autopilot_set_mode function
+ */
+void autopilot_SetModeHandler(float mode)
+{
+  if (mode == AP_MODE_KILL || mode == AP_MODE_FAILSAFE || mode == AP_MODE_HOME) {
+    // safety modes are always accessible via settings
+    autopilot_set_mode(mode);
+  } else {
+    if (radio_control.status != RC_OK &&
+        (mode == AP_MODE_NAV || mode == AP_MODE_GUIDED || mode == AP_MODE_FLIP || mode == AP_MODE_MODULE)) {
+      // without RC, only nav-like modes are accessible
+      autopilot_set_mode(mode);
+    }
+  }
+  // with RC, other modes can only be changed from the RC
+}
+
 
 void autopilot_set_mode(uint8_t new_autopilot_mode)
 {

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -67,6 +67,7 @@ extern void autopilot_init(void);
 extern void autopilot_periodic(void);
 extern void autopilot_on_rc_frame(void);
 extern void autopilot_set_mode(uint8_t new_autopilot_mode);
+extern void autopilot_SetModeHandler(float new_autopilot_mode); // handler for dl_setting
 extern void autopilot_set_motors_on(bool motors_on);
 extern void autopilot_check_in_flight(bool motors_on);
 

sw/airborne/firmwares/rotorcraft/main.c

@@ -310,7 +310,10 @@ STATIC_INLINE void failsafe_check(void)
       autopilot_mode != AP_MODE_KILL &&
       autopilot_mode != AP_MODE_HOME &&
       autopilot_mode != AP_MODE_FAILSAFE &&
-      autopilot_mode != AP_MODE_NAV) {
+      autopilot_mode != AP_MODE_NAV &&
+      autopilot_mode != AP_MODE_MODULE &&
+      autopilot_mode != AP_MODE_FLIP &&
+      autopilot_mode != AP_MODE_GUIDED) {
     autopilot_set_mode(RC_LOST_MODE);
   }
 

sw/ground_segment/joystick/gb2ivy.py

@@ -35,17 +35,17 @@ class Guidance(object):
         self.ac_id = ac_id
         self.verbose = verbose
         self._interface = None
-        self.auto2_index = None
+        self.ap_mode = None
         try:
             settings = PaparazziACSettings(self.ac_id)
         except Exception as e:
             print(e)
             return
         try:
-            self.auto2_index = settings.name_lookup['auto2'].index
+            self.ap_mode = settings.name_lookup['mode'].index
         except Exception as e:
             print(e)
-            print("auto2 setting not found, mode change not possible.")
+            print("ap_mode setting not found, mode change not possible.")
         self._interface = IvyMessagesInterface("gb2ivy")
 
     def shutdown(self):
@@ -66,24 +66,24 @@ class Guidance(object):
 
     def set_guided_mode(self):
         """
-        change auto2 mode to GUIDED.
+        change mode to GUIDED.
         """
-        if self.auto2_index is not None:
+        if self.ap_mode is not None:
             msg = PprzMessage("ground", "DL_SETTING")
             msg['ac_id'] = self.ac_id
-            msg['index'] = self.auto2_index
+            msg['index'] = self.ap_mode
             msg['value'] = 19  # AP_MODE_GUIDED
             print("Setting mode to GUIDED: %s" % msg)
             self._interface.send(msg)
 
     def set_nav_mode(self):
         """
-        change auto2 mode to NAV.
+        change mode to NAV.
         """
-        if self.auto2_index is not None:
+        if self.ap_mode is not None:
             msg = PprzMessage("ground", "DL_SETTING")
             msg['ac_id'] = self.ac_id
-            msg['index'] = self.auto2_index
+            msg['index'] = self.ap_mode
             msg['value'] = 13  # AP_MODE_NAV
             print("Setting mode to NAV: %s" % msg)
             self._interface.send(msg)