Sub: Implement crash check

ArduSub/ArduSub.cpp

@@ -191,6 +191,8 @@ 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();
 

ArduSub/Sub.cpp

@@ -50,7 +50,6 @@ Sub::Sub(void) :
           target_rangefinder_alt(0.0f),
           baro_alt(0),
           baro_climbrate(0.0f),
-          land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
           rc_throttle_control_in_filter(1.0f),
           auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
           yaw_look_at_WP_bearing(0.0f),

ArduSub/Sub.h

@@ -355,7 +355,6 @@ private:
     float target_rangefinder_alt;      // desired altitude in cm above the ground
     int32_t baro_alt;            // barometer altitude in cm above home
     float baro_climbrate;        // barometer climbrate in cm/s
-    LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
 
     // Turn counter
     int32_t quarter_turn_count;
@@ -644,7 +643,7 @@ private:
     void stabilize_run();
     bool manual_init(bool ignore_checks);
     void manual_run();
-    void crash_check();
+    void crash_check(uint32_t dt_seconds);
     void ekf_check();
     bool ekf_over_threshold();
     void failsafe_ekf_event();

ArduSub/crash_check.cpp

@@ -4,28 +4,37 @@
 #define CRASH_CHECK_TRIGGER_SEC         2       // 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()
+void Sub::crash_check(uint32_t dt_seconds)
 {
     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);
+
     // return immediately if disarmed, or crash checking disabled
     if (!motors.armed() || g.fs_crash_check == 0) {
         crash_counter = 0;
         return;
     }
 
-    // return immediately if we are not in an angle stabilize flight mode
-    if (control_mode == ACRO) {
+    // 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 (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
+    if (crash_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
         crash_counter = 0;
         return;
     }