diff --git a/ArduCopter/crash_check.pde b/ArduCopter/crash_check.pde index 301cbe8851..7289c52c30 100644 --- a/ArduCopter/crash_check.pde +++ b/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; } }