[rotorcraft] improve in_flight detection heuristic

conf/airframes/examples/quadrotor_lisa_m_2_pwm_spektrum.xml

@@ -39,6 +39,8 @@
       <define name="AHRS_GRAVITY_UPDATE_NORM_HEURISTIC" value="TRUE"/>
     </subsystem>
     <subsystem name="ins"/>
+
+    <!--define name="KILL_ON_GROUND_DETECT" value="TRUE"/-->
   </firmware>
 
   <servos driver="Pwm">

conf/flight_plans/rotorcraft_basic.xml

@@ -71,8 +71,12 @@
     </block>
     <block name="flare">
       <exception cond="NavDetectGround()" deroute="Holding point"/>
+      <exception cond="!nav_is_in_flight()" deroute="landed"/>
       <call fun="NavStartDetectGround()"/>
       <stay climb="-0.8" vmode="climb" wp="TD"/>
     </block>
+    <block name="landed">
+      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
+    </block>
   </blocks>
 </flight_plan>

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -52,7 +52,22 @@ bool_t   autopilot_power_switch;
 bool_t   autopilot_detect_ground;
 bool_t   autopilot_detect_ground_once;
 
-#define AUTOPILOT_IN_FLIGHT_TIME    40
+#define AUTOPILOT_IN_FLIGHT_TIME    20
+
+/** minimum vertical speed for in_flight condition in m/s */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_SPEED
+#define AUTOPILOT_IN_FLIGHT_MIN_SPEED 0.2
+#endif
+
+/** minimum vertical acceleration for in_flight condition in m/s^2 */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_ACCEL
+#define AUTOPILOT_IN_FLIGHT_MIN_ACCEL 2.0
+#endif
+
+/** minimum thrust for in_flight condition in pprz_t units */
+#ifndef AUTOPILOT_IN_FLIGHT_MIN_THRUST
+#define AUTOPILOT_IN_FLIGHT_MIN_THRUST 500
+#endif
 
 #ifndef AUTOPILOT_DISABLE_AHRS_KILL
 #include "subsystems/ahrs.h"
@@ -97,36 +112,6 @@ void autopilot_init(void) {
 }
 
 
-static inline void autopilot_check_in_flight_no_rc( bool_t motors_on ) {
-  if (autopilot_in_flight) {
-    if (autopilot_in_flight_counter > 0) {
-      if (stabilization_cmd[COMMAND_THRUST] == 0) {
-        autopilot_in_flight_counter--;
-        if (autopilot_in_flight_counter == 0) {
-          autopilot_in_flight = FALSE;
-        }
-      }
-      else {  /* !THROTTLE_STICK_DOWN */
-        autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
-      }
-    }
-  }
-  else { /* not in flight */
-    if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
-        motors_on) {
-      if (stabilization_cmd[COMMAND_THRUST] > 0) {
-        autopilot_in_flight_counter++;
-        if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
-          autopilot_in_flight = TRUE;
-      }
-      else { /*  THROTTLE_STICK_DOWN */
-        autopilot_in_flight_counter = 0;
-      }
-    }
-  }
-}
-
-
 void autopilot_periodic(void) {
 
   RunOnceEvery(NAV_PRESCALER, nav_periodic_task());
@@ -153,10 +138,6 @@ INFO("Using FAILSAFE_GROUND_DETECT")
     SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
   }
 
-  // when we dont have RC, check in flight by looking at throttle
-  if (radio_control.status != RC_OK) {
-    autopilot_check_in_flight_no_rc(autopilot_motors_on);
-  }
 }
 
 
@@ -255,29 +236,37 @@ void autopilot_set_mode(uint8_t new_autopilot_mode) {
 }
 
 
-static inline void autopilot_check_in_flight( bool_t motors_on ) {
+void autopilot_check_in_flight(bool_t motors_on) {
   if (autopilot_in_flight) {
     if (autopilot_in_flight_counter > 0) {
-      if (THROTTLE_STICK_DOWN()) {
+      /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
+      if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
+          (abs(stateGetSpeedNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_SPEED) &&
+          (abs(stateGetAccelNed_f()->z) < AUTOPILOT_IN_FLIGHT_MIN_ACCEL))
+      {
         autopilot_in_flight_counter--;
         if (autopilot_in_flight_counter == 0) {
           autopilot_in_flight = FALSE;
         }
       }
-      else {	/* !THROTTLE_STICK_DOWN */
+      else {  /* thrust, speed or accel not above min threshold, reset counter */
         autopilot_in_flight_counter = AUTOPILOT_IN_FLIGHT_TIME;
       }
     }
   }
-  else { /* not in flight */
+  else { /* currently not in flight */
     if (autopilot_in_flight_counter < AUTOPILOT_IN_FLIGHT_TIME &&
-        motors_on) {
-      if (!THROTTLE_STICK_DOWN()) {
+        motors_on)
+    {
+      /* if thrust above min threshold, assume in_flight.
+       * Don't check for velocity and acceleration above threshold here...
+       */
+      if (stabilization_cmd[COMMAND_THRUST] > AUTOPILOT_IN_FLIGHT_MIN_THRUST) {
         autopilot_in_flight_counter++;
         if (autopilot_in_flight_counter == AUTOPILOT_IN_FLIGHT_TIME)
           autopilot_in_flight = TRUE;
       }
-      else { /*  THROTTLE_STICK_DOWN */
+      else { /* currently not in_flight and thrust below threshold, reset counter */
         autopilot_in_flight_counter = 0;
       }
     }
@@ -330,8 +319,6 @@ void autopilot_on_rc_frame(void) {
     autopilot_arming_check_motors_on();
     kill_throttle = ! autopilot_motors_on;
 
-    autopilot_check_in_flight(autopilot_motors_on);
-
     guidance_v_read_rc();
     guidance_h_read_rc(autopilot_in_flight);
   }

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -67,6 +67,7 @@ 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_set_motors_on(bool_t motors_on);
+extern void autopilot_check_in_flight(bool_t motors_on);
 
 extern bool_t autopilot_detect_ground;
 extern bool_t autopilot_detect_ground_once;

sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c

@@ -220,13 +220,16 @@ void guidance_v_run(bool_t in_flight) {
         run_hover_loop(in_flight);
       }
       else if (vertical_mode == VERTICAL_MODE_CLIMB) {
+        guidance_v_z_sp = stateGetPositionNed_i()->z;
         guidance_v_zd_sp = -nav_climb;
         gv_update_ref_from_zd_sp(guidance_v_zd_sp);
-        nav_flight_altitude = -guidance_v_z_sp;
         run_hover_loop(in_flight);
       }
       else if (vertical_mode == VERTICAL_MODE_MANUAL) {
-        guidance_v_z_sp = -nav_flight_altitude; // For display only
+        guidance_v_z_sp = stateGetPositionNed_i()->z;
+        guidance_v_zd_sp = stateGetSpeedNed_i()->z;
+        GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0);
+        guidance_v_z_sum_err = 0;
         guidance_v_delta_t = nav_throttle;
       }
 #if NO_RC_THRUST_LIMIT

sw/airborne/firmwares/rotorcraft/main.c

@@ -233,6 +233,8 @@ STATIC_INLINE void failsafe_check( void ) {
     autopilot_set_mode(AP_MODE_FAILSAFE);
   }
 #endif
+
+  autopilot_check_in_flight(autopilot_motors_on);
 }
 
 STATIC_INLINE void main_event( void ) {

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -341,4 +341,11 @@ bool_t nav_detect_ground(void) {
   return TRUE;
 }
 
+bool_t nav_is_in_flight(void) {
+  if (autopilot_in_flight)
+    return TRUE;
+  else
+    return FALSE;
+}
+
 void nav_home(void) {}

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -81,6 +81,7 @@ unit_t nav_reset_alt( void ) __attribute__ ((unused));
 void nav_periodic_task(void);
 void nav_move_waypoint(uint8_t wp_id, struct EnuCoor_i * new_pos);
 bool_t nav_detect_ground(void);
+bool_t nav_is_in_flight(void);
 
 void nav_home(void);