Rotorcraft static ap add option to use failsafe throttle setting (#3453)

conf/airframes/tudelft/rotwing_7kg_common.xml

@@ -2,7 +2,15 @@
 
 <airframe>
 
-    <section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
+    <section name="AP_FAILSAFE">
+        <!-- <define name="NO_GPS_LOST_WITH_DATALINK_TIME"       value="20"/> --> <!-- Only commented out for navy testing -->
+        <define name="NO_GPS_LOST_WITH_RC_VALID"            value="FALSE"/> <!-- Set to FALSE for navy testing -->
+        <define name="RC_LOST_MODE"                         value="AP_MODE_NAV"/> 
+        <define name="GPS_FIX_TIMEOUT"                      value="5"/> 
+        <define name="FAILSAFE_DESCENT_SPEED"               value="1.0"/> <!-- Does not work as expected with AP static -->
+    </section>
+
+    <section name="CTRL_EFF_SCHED" prefix="ROTWING_EFF_SCHED_">
         <define name="IXX_BODY"              value="0.04780"/>
         <define name="IYY_BODY"              value="0.7546"/>
         <define name="IZZ"                   value="0.9752"/>

conf/airframes/tudelft/rotwing_v3b.xml

@@ -242,6 +242,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5800"/>
+
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

conf/airframes/tudelft/rotwing_v3d.xml

@@ -236,6 +236,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5300"/>
+
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

conf/airframes/tudelft/rotwing_v3e.xml

@@ -236,6 +236,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5300"/>
+        
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

conf/airframes/tudelft/rotwing_v3f.xml

@@ -234,6 +234,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5300"/>
+        
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

conf/airframes/tudelft/rotwing_v3g.xml

@@ -236,6 +236,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5300"/>
+        
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

conf/airframes/tudelft/rotwing_v3h.xml

@@ -236,6 +236,9 @@
     </section>
 
     <section name="CALIBRATION">
+        <!-- Throttle value in AP_MODE_FAILSAFE -->
+        <define name="FAILSAFE_THROTTLE"                    value="5300"/>
+        
         <!-- Voltage and current measurements -->
         <define name="VoltageOfAdc(adc)" value="((3.3f/65536.0f) * 11.98389 * adc)"/>
         <define name="VBoardOfAdc(adc)" value="((3.3f/65536.0f) * 1.89036 * adc)"/>

sw/airborne/firmwares/rotorcraft/autopilot_static.c

@@ -162,7 +162,11 @@ void autopilot_static_periodic(void)
   switch (autopilot.mode) {
     case AP_MODE_FAILSAFE:
 #ifndef KILL_AS_FAILSAFE
+#ifdef FAILSAFE_THROTTLE
+      thrust_sp = th_sp_from_thrust_i(FAILSAFE_THROTTLE, THRUST_AXIS_Z);
+#else
       thrust_sp = guidance_v_run(autopilot_in_flight());
+#endif
       stab_sp = stabilization_get_failsafe_sp();
       stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd);
       SetRotorcraftCommands(stabilization.cmd, autopilot.in_flight, autopilot.motors_on);

sw/airborne/modules/rotwing_drone/rotwing_state.c

@@ -188,7 +188,10 @@ void rotwing_state_periodic(void)
 
   /* Override modes if flying with RC */
   rotwing_state.state = rotwing_state.nav_state;
-  if(guidance_h.mode == GUIDANCE_H_MODE_NONE) {
+  if (autopilot.mode == AP_MODE_FAILSAFE) {
+    rotwing_state.state = ROTWING_STATE_FORCE_HOVER;
+  } 
+  else if (guidance_h.mode == GUIDANCE_H_MODE_NONE) {
     // Kill mode
     if(stabilization.mode == STABILIZATION_MODE_NONE) {
       rotwing_state.state = ROTWING_STATE_FORCE_HOVER;