You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
61 lines
2.4 KiB
61 lines
2.4 KiB
/// -*- 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 // 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 |
|
#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 |
|
// 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 |
|
static int32_t baro_alt_prev; |
|
|
|
// 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; |
|
} |
|
|
|
// return immediately if we are not in an angle stabilize flight mode or we are flipping |
|
if (control_mode == ACRO || control_mode == FLIP) { |
|
inverted_count = 0; |
|
return; |
|
} |
|
|
|
// check angles |
|
int32_t lean_max = aparm.angle_max + CRASH_CHECK_ANGLE_DEVIATION_CD; |
|
if (labs(ahrs.roll_sensor) > lean_max || labs(ahrs.pitch_sensor) > lean_max) { |
|
inverted_count++; |
|
|
|
// 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")); |
|
// disarm motors |
|
init_disarm_motors(); |
|
} |
|
}else{ |
|
// we are not inverted so reset counter |
|
inverted_count = 0; |
|
} |
|
}
|
|
|