Plane: reworked is_flying

add crash detection, allow disengage via param CRASH_DETECT
improved is_flying behavior
take off, landing and hard-landing improvements
add stillness check to is_flying and log it
minimum airspeed is determined ARSPD_FBW_MIN*0.75

ArduPlane/ArduPlane.cpp

@@ -74,6 +74,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] PROGMEM = {
     { SCHED_TASK(frsky_telemetry_send),  10,    100 },
 #endif
     { SCHED_TASK(terrain_update),         5,    500 },
+    { SCHED_TASK(update_is_flying_5Hz),  10,    100 },
 };
 
 void Plane::setup() 
@@ -303,14 +304,13 @@ void Plane::one_second_loop()
 
     update_aux();
 
-    // determine if we are flying or not
-    determine_is_flying();
-
     // update notify flags
     AP_Notify::flags.pre_arm_check = arming.pre_arm_checks(false);
     AP_Notify::flags.pre_arm_gps_check = true;
     AP_Notify::flags.armed = arming.is_armed() || arming.arming_required() == AP_Arming::NO;
 
+    crash_detection_update();
+
 #if AP_TERRAIN_AVAILABLE
     if (should_log(MASK_LOG_GPS)) {
         terrain.log_terrain_data(DataFlash);
@@ -856,43 +856,111 @@ void Plane::update_flight_stage(void)
   Do we think we are flying?
   Probabilistic method where a bool is low-passed and considered a probability.
 */
-void Plane::determine_is_flying(void)
+void Plane::update_is_flying_5Hz(void)
 {
     float aspeed;
-    bool isFlyingBool;
+    bool is_flying_bool;
+    uint32_t now_ms = hal.scheduler->millis();
 
-    bool airspeedMovement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= 5);
+    uint32_t ground_speed_thresh_cm = (g.min_gndspeed_cm > 0) ? ((uint32_t)(g.min_gndspeed_cm*0.9f)) : 500;
+    bool gps_confirmed_movement = (gps.status() >= AP_GPS::GPS_OK_FIX_3D) &&
+                                    (gps.ground_speed_cm() >= ground_speed_thresh_cm);
 
-    // If we don't have a GPS lock then don't use GPS for this test
-    bool gpsMovement = (gps.status() < AP_GPS::GPS_OK_FIX_2D ||
-                        gps.ground_speed() >= 5);
+    // airspeed at least 75% of stall speed?
+    bool airspeed_movement = ahrs.airspeed_estimate(&aspeed) && (aspeed >= (aparm.airspeed_min*0.75f));
 
+    if (ins.is_still()) {
+        // if motionless, we can't possibly be flying. This is a very strict test
+        is_flying_bool = false;
+    }
+    else if(arming.is_armed()) {
+        // when armed assuming flying and we need overwhelming evidence that we ARE NOT flying
 
-    if (hal.util->get_soft_armed()) {
-        // when armed, we need overwhelming evidence that we ARE NOT flying
-        isFlyingBool = airspeedMovement || gpsMovement;
+        // short drop-outs of GPS are common during flight due to banking which points the antenna in different directions
+        bool gps_lost_recently = (gps.last_fix_time_ms() > 0) && // we have locked to GPS before
+                        (gps.status() < AP_GPS::GPS_OK_FIX_2D) && // and it's lost now
+                        (now_ms - gps.last_fix_time_ms() < 5000); // but it wasn't that long ago (<5s)
 
-        /*
-          make is_flying() more accurate for landing approach
-         */
-        if (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_APPROACH &&
-            fabsf(auto_state.sink_rate) > 0.2f) {
-            isFlyingBool = true;
+        if ((auto_state.last_flying_ms > 0) && gps_lost_recently) {
+            // we've flown before, remove GPS constraints temporarily and only use airspeed
+            is_flying_bool = airspeed_movement; // moving through the air
+        } else {
+            // we've never flown yet, require good GPS movement
+            is_flying_bool = airspeed_movement || // moving through the air
+                                gps_confirmed_movement; // locked and we're moving
+        }
+
+        if (control_mode == AUTO) {
+            /*
+              make is_flying() more accurate during various auto modes
+             */
+
+            switch (flight_stage)
+            {
+            case AP_SpdHgtControl::FLIGHT_TAKEOFF:
+                // while on the ground, an uncalibrated airspeed sensor can drift to 7m/s so
+                // ensure we aren't showing a false positive. If the throttle is suppressed
+                // we are definitely not flying, or at least for not much longer!
+                if (throttle_suppressed) {
+                    is_flying_bool = false;
+                    auto_state.is_crashed = false;
+                }
+                break;
+
+            case AP_SpdHgtControl::FLIGHT_NORMAL:
+                // TODO: detect ground impacts
+                break;
+
+            case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
+                // TODO: detect ground impacts
+                if (fabsf(auto_state.sink_rate) > 0.2f) {
+                    is_flying_bool = true;
+                }
+                break;
+
+            case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
+            default:
+                break;
+            } // switch
         }
     } else {
-        // when disarmed, we need overwhelming evidence that we ARE flying
-        isFlyingBool = airspeedMovement && gpsMovement;
+        // when disarmed assume not flying and need overwhelming evidence that we ARE flying
+        is_flying_bool = airspeed_movement && gps_confirmed_movement;
+
+        if ((control_mode == AUTO) &&
+            ((flight_stage == AP_SpdHgtControl::FLIGHT_TAKEOFF) ||
+             (flight_stage == AP_SpdHgtControl::FLIGHT_LAND_FINAL)) ) {
+            is_flying_bool = false;
+        }
     }
 
     // low-pass the result.
-    isFlyingProbability = (0.6f * isFlyingProbability) + (0.4f * (float)isFlyingBool);
+    // coef=0.15f @ 5Hz takes 3.0s to go from 100% down to 10% (or 0% up to 90%)
+    isFlyingProbability = (0.85f * isFlyingProbability) + (0.15f * (float)is_flying_bool);
 
     /*
       update last_flying_ms so we always know how long we have not
       been flying for. This helps for crash detection and auto-disarm
      */
-    if (is_flying()) {
-        auto_state.last_flying_ms = millis();
+    bool new_is_flying = is_flying();
+    static bool previous_is_flying = false;
+
+    // we are flying, note the time
+    if (new_is_flying) {
+
+        auto_state.last_flying_ms = now_ms;
+
+        if ((control_mode == AUTO) &&
+            ((auto_state.started_flying_in_auto_ms == 0) || !previous_is_flying) ) {
+
+            // We just started flying, note that time also
+            auto_state.started_flying_in_auto_ms = now_ms;
+        }
+    }
+    previous_is_flying = new_is_flying;
+
+    if (should_log(MASK_LOG_MODE)) {
+        Log_Write_Status();
     }
 }
 
@@ -912,6 +980,120 @@ bool Plane::is_flying(void)
     return (isFlyingProbability >= 0.9f);
 }
 
+/*
+ * Determine if we have crashed
+ */
+void Plane::crash_detection_update(void)
+{
+    static uint32_t crash_timer_ms = 0;
+    static bool checkHardLanding = false;
+
+    if (control_mode != AUTO)
+    {
+        // crash detection is only available in AUTO mode
+        crash_timer_ms = 0;
+        return;
+    }
+
+    uint32_t now_ms = hal.scheduler->millis();
+    bool auto_launch_detected;
+    bool crashed_near_land_waypoint = false;
+    bool crashed = false;
+    bool been_auto_flying = (auto_state.started_flying_in_auto_ms > 0) &&
+                            (now_ms - auto_state.started_flying_in_auto_ms >= 2500);
+
+    if (!is_flying())
+    {
+        switch (flight_stage)
+        {
+        case AP_SpdHgtControl::FLIGHT_TAKEOFF:
+            auto_launch_detected = !throttle_suppressed && (g.takeoff_throttle_min_accel > 0);
+
+            if (been_auto_flying || // failed hand launch
+                auto_launch_detected) { // threshold of been_auto_flying may not be met on auto-launches
+
+                // has launched but is no longer flying. That's a crash on takeoff.
+                crashed = true;
+            }
+            break;
+
+        case AP_SpdHgtControl::FLIGHT_NORMAL:
+            if (been_auto_flying) {
+                crashed = true;
+            }
+            // TODO: handle auto missions without NAV_TAKEOFF mission cmd
+            break;
+
+        case AP_SpdHgtControl::FLIGHT_LAND_APPROACH:
+            if (been_auto_flying) {
+                crashed = true;
+            }
+            // when altitude gets low, we automatically progress to FLIGHT_LAND_FINAL
+            // so ground crashes most likely can not be triggered from here. However,
+            // a crash into a tree, for example, would be.
+            break;
+
+        case AP_SpdHgtControl::FLIGHT_LAND_FINAL:
+            // We should be nice and level-ish in this flight stage. If not, we most
+            // likely had a crazy landing. Throttle is inhibited already at the flare
+            // but go ahead and notify GCS and perform any additional post-crash actions.
+            // Declare a crash if we are oriented more that 60deg in pitch or roll
+            if (been_auto_flying &&
+                !checkHardLanding && // only check once
+                (fabsf(ahrs.roll_sensor) > 6000 || fabsf(ahrs.pitch_sensor) > 6000)) {
+                crashed = true;
+
+                // did we "crash" within 75m of the landing location? Probably just a hard landing
+                crashed_near_land_waypoint =
+                        get_distance(current_loc, mission.get_current_nav_cmd().content.location) < 75;
+
+                // trigger hard landing event right away, or never again. This inhibits a false hard landing
+                // event when, for example, a minute after a good landing you pick the plane up and
+                // this logic is still running and detects the plane is on its side as you carry it.
+                crash_timer_ms = now_ms + 2500;
+            }
+            checkHardLanding = true;
+            break;
+
+        default:
+            break;
+        } // switch
+    } else {
+        checkHardLanding = false;
+    }
+
+    if (!crashed) {
+        // reset timer
+        crash_timer_ms = 0;
+        auto_state.is_crashed = false;
+
+    } else if (crash_timer_ms == 0) {
+        // start timer
+        crash_timer_ms = now_ms;
+
+    } else if ((now_ms - crash_timer_ms >= 2500) && !auto_state.is_crashed) {
+        auto_state.is_crashed = true;
+
+        if (g.crash_detection_enable == CRASH_DETECT_ACTION_BITMASK_DISABLED) {
+            if (crashed_near_land_waypoint) {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected - no action taken"));
+            } else {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected - no action taken"));
+            }
+        }
+        else {
+            if (g.crash_detection_enable & CRASH_DETECT_ACTION_BITMASK_DISARM) {
+                disarm_motors();
+            }
+            auto_state.land_complete = true;
+            if (crashed_near_land_waypoint) {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Hard Landing Detected"));
+            } else {
+                gcs_send_text_P(SEVERITY_HIGH, PSTR("Crash Detected"));
+            }
+        }
+    }
+}
 
 #if OPTFLOW == ENABLED
 // called at 50hz

ArduPlane/Log.cpp

@@ -318,6 +318,8 @@ struct PACKED log_Status {
     float is_flying_probability;
     uint8_t armed;
     uint8_t safety;
+    bool is_crashed;
+    bool is_still;
 };
 
 void Plane::Log_Write_Status()
@@ -329,7 +331,9 @@ void Plane::Log_Write_Status()
         ,is_flying_probability : isFlyingProbability
         ,armed       : hal.util->get_soft_armed()
         ,safety      : hal.util->safety_switch_state()
-    };
+        ,is_crashed  : auto_state.is_crashed
+        ,is_still    : plane.ins.is_still()
+        };
 
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
@@ -484,7 +488,7 @@ static const struct LogStructure log_structure[] PROGMEM = {
     { LOG_ATRP_MSG, sizeof(AP_AutoTune::log_ATRP),
       "ATRP", "QBBcfff",  "TimeUS,Type,State,Servo,Demanded,Achieved,P" },
     { LOG_STATUS_MSG, sizeof(log_Status),
-      "STAT", "QBfBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety" },
+      "STAT", "QBfBBBB",  "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still" },
 #if OPTFLOW == ENABLED
     { LOG_OPTFLOW_MSG, sizeof(log_Optflow),
       "OF",   "QBffff",   "TimeUS,Qual,flowX,flowY,bodyX,bodyY" },

ArduPlane/Parameters.cpp

@@ -973,6 +973,14 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(trim_rc_at_start,     "TRIM_RC_AT_START",    0), 
 
+    // @Param: CRASH_DETECT
+    // @DisplayName: Crash Detection
+    // @Description: Automatically detect a crash during AUTO flight and perform the bitmask selected action(s). Disarm will turn off motor for saftey and to help against burning out ESC and motor. Setting the mode to manual will help save the servos from burning out by overexerting if the aircraft crashed in an odd orientation such as upsidedown.
+    // @Values: 0:Disabled,1:Disarm
+    // @Bitmask: 0:Disarm
+    // @User: Advanced
+    GSCALAR(crash_detection_enable,         "CRASH_DETECT",   0),
+
     // barometer ground calibration. The GND_ prefix is chosen for
     // compatibility with previous releases of ArduPlane
     // @Group: GND_

ArduPlane/Parameters.h

@@ -139,6 +139,7 @@ public:
         k_param_rudder_only,
         k_param_gcs3,            // 93
         k_param_gcs_pid_mask,
+        k_param_crash_detection_enable,
 
         // 100: Arming parameters
         k_param_arming = 100,
@@ -338,6 +339,7 @@ public:
     AP_Int8  rtl_autoland;
 
     AP_Int8  trim_rc_at_start;
+    AP_Int8  crash_detection_enable;
 
     // Feed-forward gains
     //

ArduPlane/Plane.h

@@ -433,6 +433,9 @@ private:
         // movement until altitude is reached
         bool idle_mode:1;
 
+        // crash detection
+        bool is_crashed:1;
+
         // used to 'wiggle' servos in idle mode to prevent them freezing
         // at high altitudes
         uint8_t idle_wiggle_stage;
@@ -471,6 +474,9 @@ private:
 
         // once landed, post some landing statistics to the GCS
         bool post_landing_stats;
+
+        // time stamp of when we start flying while in auto mode in milliseconds
+        uint32_t started_flying_in_auto_ms;
     } auto_state;
 
     // true if we are in an auto-throttle mode, which means
@@ -865,7 +871,8 @@ private:
     void set_servos_idle(void);
     void set_servos();
     void update_aux();
-    void determine_is_flying(void);
+    void update_is_flying_5Hz(void);
+    void crash_detection_update(void);
     void gcs_send_text_fmt(const prog_char_t *fmt, ...);
     void handle_auto_mode(void);
     void calc_throttle();

ArduPlane/commands_logic.cpp

@@ -22,6 +22,9 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
         // except in a takeoff
         auto_state.takeoff_complete = true;
 
+        // if we are still executing mission commands then we must be traveling around still
+        auto_state.is_crashed = false;
+
         // if a go around had been commanded, clear it now.
         auto_state.commanded_go_around = false;
 
@@ -39,6 +42,7 @@ bool Plane::start_command(const AP_Mission::Mission_Command& cmd)
     switch(cmd.id) {
 
     case MAV_CMD_NAV_TAKEOFF:
+        auto_state.is_crashed = false;
         do_takeoff(cmd);
         break;
 

ArduPlane/defines.h

@@ -185,4 +185,9 @@ enum {
     ATT_CONTROL_APMCONTROL = 1
 };
 
+enum {
+    CRASH_DETECT_ACTION_BITMASK_DISABLED = 0,
+    CRASH_DETECT_ACTION_BITMASK_DISARM = (1<<0),
+    // note: next enum will be (1<<1), then (1<<2), then (1<<3)
+};
 #endif // _DEFINES_H

ArduPlane/system.cpp

@@ -347,6 +347,9 @@ void Plane::set_mode(enum FlightMode mode)
     // zero locked course
     steer_state.locked_course_err = 0;
 
+    // reset crash detection
+    auto_state.is_crashed = false;
+
     // set mode
     previous_mode = control_mode;
     control_mode = mode;
@@ -484,6 +487,7 @@ void Plane::exit_mode(enum FlightMode mode)
         if (mission.state() == AP_Mission::MISSION_RUNNING) {
             mission.stop();
         }
+        auto_state.started_flying_in_auto_ms = 0;
     }
 }