From 2326b2e5f58ebee261d96e5e8ba53da6223fec7b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 29 Oct 2013 22:11:48 +0900 Subject: [PATCH] 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 | 3 ++- ArduCopter/crash_check.pde | 43 ++++++++++++++++++++++++++++++++++++++ ArduCopter/defines.h | 6 ++++-- 3 files changed, 49 insertions(+), 3 deletions(-) create mode 100644 ArduCopter/crash_check.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4b32ce736e..8705b2005d 100644 --- a/ArduCopter/ArduCopter.pde +++ b/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(); diff --git a/ArduCopter/crash_check.pde b/ArduCopter/crash_check.pde new file mode 100644 index 0000000000..301cbe8851 --- /dev/null +++ b/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(); + } + } +} diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 879a8f0f40..80fc09aabb 100644 --- a/ArduCopter/defines.h +++ b/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