mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 07:58:28 -04:00
2326b2e5f5
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
44 lines
1.6 KiB
Plaintext
44 lines
1.6 KiB
Plaintext
/// -*- 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();
|
|
}
|
|
}
|
|
}
|