Copter: add alt and throttle checks to crash detector

ArduCopter/crash_check.pde

@@ -2,10 +2,13 @@
 
 // Code to detect a crash main ArduCopter code
 #ifndef CRASH_CHECK_ITERATIONS_MAX
- # define CRASH_CHECK_ITERATIONS_MAX  20          // 1 second (ie. 10 iterations at 10hz) inverted indicates a crash
+ # define CRASH_CHECK_ITERATIONS_MAX        20      // 2 second (ie. 10 iterations at 10hz) inverted indicates a crash
 #endif
 #ifndef CRASH_CHECK_ANGLE_DEVIATION_CD
- # define CRASH_CHECK_ANGLE_DEVIATION_CD  2000    // 20 degrees beyond angle max is signal we are inverted
+ # define CRASH_CHECK_ANGLE_DEVIATION_CD    2000    // 20 degrees beyond angle max is signal we are inverted
+#endif
+#ifndef CRASH_CHECK_ALT_CHANGE_LIMIT_CM
+ # define CRASH_CHECK_ALT_CHANGE_LIMIT_CM   50      // baro altitude must not change by more than 50cm
 #endif
 
 // crash_check - disarms motors if a crash has been detected
@@ -14,9 +17,10 @@
 void crash_check()
 {
     static uint8_t inverted_count;  // number of iterations we have been inverted
+    static int32_t baro_alt_prev;
 
-    // return immediately if motors are not armed
-    if (!motors.armed()) {
+    // return immediately if motors are not armed or pilot's throttle is above zero
+    if (!motors.armed() || (g.rc_3.control_in != 0 && !failsafe.radio)) {
         inverted_count = 0;
         return;
     }
@@ -31,13 +35,27 @@ void crash_check()
     int32_t lean_max = g.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD;
     if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) {
         inverted_count++;
-        if (inverted_count >= CRASH_CHECK_ITERATIONS_MAX) {
+
+        // if we have just become inverted record the baro altitude
+        if (inverted_count == 1) {
+            baro_alt_prev = baro_alt;
+
+        // exit if baro altitude change indicates we are moving (probably falling)
+        }else if (labs(baro_alt - baro_alt_prev) > CRASH_CHECK_ALT_CHANGE_LIMIT_CM) {
+            inverted_count = 0;
+            return;
+
+        // check if inverted for 2 seconds
+        }else if (inverted_count >= CRASH_CHECK_ITERATIONS_MAX) {
             // 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_P(SEVERITY_HIGH,PSTR("Crash: disarming"));
+            gcs_send_text_P(SEVERITY_HIGH,PSTR("Crash: Disarming"));
             // disarm motors
             init_disarm_motors();
         }
+    }else{
+        // we are not inverted so reset counter
+        inverted_count = 0;
     }
 }