Copter: crash checker

Crash is determined to have happened when the copter is 20deg more than
the ANGLE_MAX parameter continuously for more than 2 seconds
Not activated when in ACRO mode or while flipping

ArduCopter/ArduCopter.pde

@@ -871,6 +871,7 @@ static const AP_Scheduler::Task scheduler_tasks[] PROGMEM = {
     { barometer_accumulate,  2,     250 },
     { update_notify,         2,     100 },
     { one_hz_loop,         100,     420 },
+    { crash_check,          10,      20 },
     { gcs_check_input,	     2,     550 },
     { gcs_send_heartbeat,  100,     150 },
     { gcs_send_deferred,     2,     720 },
@@ -976,7 +977,7 @@ void loop()
 {
     // wait for an INS sample
     if (!ins.wait_for_sample(1000)) {
-        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_INS_DELAY);
+        Log_Write_Error(ERROR_SUBSYSTEM_MAIN, ERROR_CODE_MAIN_INS_DELAY);
         return;
     }
     uint32_t timer = micros();

ArduCopter/crash_check.pde

@@ -0,0 +1,43 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+// 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
+#endif
+#ifndef CRASH_CHECK_ANGLE_DEVIATION_CD
+ # define CRASH_CHECK_ANGLE_DEVIATION_CD  2000    // 20 degrees beyond angle max is signal we are inverted
+#endif
+
+// 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
+// should be called at 10hz
+void crash_check()
+{
+    static uint8_t inverted_count;  // number of iterations we have been inverted
+
+    // return immediately if motors are not armed
+    if (!motors.armed()) {
+        inverted_count = 0;
+        return;
+    }
+
+    // return immediately if we are not in an angle stabilize flight mode or we are flipping
+    if (control_mode == ACRO || ap.do_flip) {
+        inverted_count = 0;
+        return;
+    }
+
+    // check angles
+    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) {
+            // 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"));
+            // disarm motors
+            init_disarm_motors();
+        }
+    }
+}

ArduCopter/defines.h

@@ -445,6 +445,7 @@ enum ap_message {
 #define ERROR_SUBSYSTEM_FAILSAFE_FENCE      9
 #define ERROR_SUBSYSTEM_FLIGHT_MODE         10
 #define ERROR_SUBSYSTEM_GPS                 11
+#define ERROR_SUBSYSTEM_CRASH_CHECK         12
 // general error codes
 #define ERROR_CODE_ERROR_RESOLVED           0
 #define ERROR_CODE_FAILED_TO_INITIALISE     1
@@ -458,8 +459,9 @@ enum ap_message {
 // subsystem specific error codes -- gps
 #define ERROR_CODE_GPS_GLITCH               2
 // subsystem specific error codes -- main
-#define ERROR_CODE_INS_DELAY                1
+#define ERROR_CODE_MAIN_INS_DELAY           1
-
+// subsystem specific error codes -- crash checker
+#define ERROR_CODE_CRASH_CHECK_CRASH        1
 
 
 #endif // _DEFINES_H