[rotorcraft] improve in_flight detection heuristic

Simply checks if thrust, speed and acceleration are above a threshold. Does not rely on RC thrust command anymore, but only on actual thrust command. Not tested on real vehicle at all so far... Attempt to improve issue #201 and replaces #468
paparazzi · Jul 2, 2013 · 6230a36 · 6230a36
1 parent fce87d0
commit 6230a36
Show file tree

Hide file tree

Showing 3 changed files with 34 additions and 44 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/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 50
+#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);
   }

diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/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;

diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/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 ) {