Sub: Implement crash failsafe check/action

ArduSub/ArduSub.cpp

@@ -185,8 +185,6 @@ void Sub::fast_loop()
     // check if ekf has reset target heading
     check_ekf_yaw_reset();
 
-    crash_check(MAIN_LOOP_SECONDS);
-
     // run the attitude controllers
     update_flight_mode();
 
@@ -216,6 +214,8 @@ void Sub::fifty_hz_loop()
     // check pilot input failsafe
     failsafe_manual_control_check();
 
+    failsafe_crash_check();
+
     // Update servo output
     RC_Channels::set_pwm_all();
     SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);

ArduSub/Parameters.cpp

@@ -306,9 +306,9 @@ const AP_Param::Info Sub::var_info[] = {
     // @Param: FS_CRASH_CHECK
     // @DisplayName: Crash check enable
     // @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.
-    // @Values: 0:Disabled
+    // @Values: 0:Disabled,1:Warn only,2:Disarm
     // @User: Advanced
-    GSCALAR(fs_crash_check, "FS_CRASH_CHECK",    0),
+    GSCALAR(fs_crash_check, "FS_CRASH_CHECK",    FS_CRASH_DISABLED),
 
     // @Param: JS_GAIN_DEFAULT
     // @DisplayName: Default gain at boot

ArduSub/Sub.h

@@ -279,6 +279,7 @@ private:
         uint8_t leak                 : 1; // true if leak recently detected
         uint8_t internal_pressure    : 1; // true if internal pressure is over threshold
         uint8_t internal_temperature : 1; // true if temperature is over threshold
+        uint8_t crash                : 1; // true if we are crashed
         uint32_t last_leak_warn_ms;      // last time a leak warning was sent to gcs
         uint32_t last_gcs_warn_ms;
 
@@ -287,6 +288,7 @@ private:
         uint32_t terrain_first_failure_ms;  // the first time terrain data access failed - used to calculate the duration of the failure
         uint32_t terrain_last_failure_ms;   // the most recent time terrain data access failed
         uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs
+        uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs
     } failsafe;
 
     // sensor health for logging
@@ -618,7 +620,7 @@ private:
     void stabilize_run();
     bool manual_init(bool ignore_checks);
     void manual_run();
-    void crash_check(uint32_t dt_seconds);
+    void failsafe_crash_check();
     void ekf_check();
     bool ekf_over_threshold();
     void failsafe_ekf_event();

ArduSub/defines.h

@@ -304,6 +304,11 @@ enum RTLState {
 #define FS_TEMP_DISABLED        0
 #define FS_TEMP_WARN_ONLY       1
 
+// Crash failsafe options
+#define FS_CRASH_DISABLED  0
+#define FS_CRASH_WARN_ONLY 1
+#define FS_CRASH_DISARM    2
+
 // Terrain failsafe actions for AUTO mode
 #define FS_TERRAIN_DISARM       0
 #define FS_TERRAIN_HOLD         1

ArduSub/failsafe.cpp

@@ -287,62 +287,61 @@ void Sub::failsafe_gcs_check()
     }
 }
 
-// Code to detect a crash
-#define CRASH_CHECK_TRIGGER_SEC         2       // 2 seconds inverted indicates a crash
+#define CRASH_CHECK_TRIGGER_MS          2000    // 2 seconds inverted indicates a crash
 #define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f   // 30 degrees beyond angle max is signal we are inverted
-#define CRASH_CHECK_ACCEL_MAX           3.0f    // vehicle must be accelerating less than 3m/s/s to be considered crashed
-#define CRASH_CHECK_ACCEL_LPF_CUTOFF    1.0f    // LPF cutoff frequency for acceleration vector
 
-// crash_check - disarms motors if a crash has been detected
-// crashes are detected by the vehicle being more than 20 degrees beyond it's angle limits continuously for more than 1 second
-// called at MAIN_LOOP_RATE
-void Sub::crash_check(uint32_t dt_seconds)
+// Check for a crash
+// The vehicle is considered crashed if the angle error exceeds a specified limit for more than 2 seconds
+void Sub::failsafe_crash_check()
 {
-    static uint16_t crash_counter;  // number of iterations vehicle may have been crashed
-
-    static LowPassFilterVector3f crash_accel_ef_filter(CRASH_CHECK_ACCEL_LPF_CUTOFF); // acceleration for checking if crashed
-
-    // update 1hz filtered acceleration
-    Vector3f accel_ef = ahrs.get_accel_ef_blended();
-    accel_ef.z += GRAVITY_MSS;
-
-    crash_accel_ef_filter.apply(accel_ef, dt_seconds);
+    static uint32_t last_crash_check_pass_ms;
+    uint32_t tnow = AP_HAL::millis();
 
     // return immediately if disarmed, or crash checking disabled
-    if (!motors.armed() || g.fs_crash_check == 0) {
-        crash_counter = 0;
+    if (!motors.armed() || g.fs_crash_check == FS_CRASH_DISABLED) {
+        last_crash_check_pass_ms = tnow;
+        failsafe.crash = false;
         return;
     }
 
     // return immediately if we are not in an angle stabilized flight mode
     if (control_mode == ACRO || control_mode == MANUAL) {
-        crash_counter = 0;
-        return;
-    }
-
-    // vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
-    if (crash_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
-        crash_counter = 0;
+        last_crash_check_pass_ms = tnow;
+        failsafe.crash = false;
         return;
     }
 
     // check for angle error over 30 degrees
     const float angle_error = attitude_control.get_att_error_angle_deg();
     if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
-        crash_counter = 0;
+        last_crash_check_pass_ms = tnow;
+        failsafe.crash = false;
         return;
     }
 
-    // we may be crashing
-    crash_counter++;
+    if (tnow < last_crash_check_pass_ms + CRASH_CHECK_TRIGGER_MS) {
+        return;
+    }
 
-    // check if crashing for 2 seconds
-    if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
+    // Conditions met, we are in failsafe
+
+    // Send warning to GCS
+    if (tnow > failsafe.last_crash_warn_ms + 20000) {
+        failsafe.last_crash_warn_ms = tnow;
+        gcs_send_text(MAV_SEVERITY_WARNING,"Crash detected");
+    }
+
+    // Only perform failsafe action once
+    if (failsafe.crash) {
+        return;
+    }
+
+    failsafe.crash = true;
     // log an error in the dataflash
     Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
-        // send message to gcs
-        gcs_send_text(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
+
     // disarm motors
+    if (g.fs_crash_check == FS_CRASH_DISARM) {
         init_disarm_motors();
     }
 }