ardupilot/ArduCopter/crash_check.pde

203 lines
7.2 KiB
Plaintext
Raw Normal View History

/// -*- 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;
#if PARACHUTE == ENABLED
// check parachute
parachute_check();
#endif
// return immediately if motors are not armed or pilot's throttle is above zero
if (!motors.armed() || (!ap.throttle_zero && !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;
}
}
#if PARACHUTE == ENABLED
// Code to detect a crash main ArduCopter code
#ifndef PARACHUTE_CHECK_ITERATIONS_MAX
# define PARACHUTE_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of loss of control triggers the parachute
#endif
#ifndef PARACHUTE_CHECK_ANGLE_DEVIATION_CD
# define PARACHUTE_CHECK_ANGLE_DEVIATION_CD 3000 // 30 degrees off from target indicates a loss of control
#endif
// parachute_check - disarms motors and triggers the parachute if serious loss of control has been detected
// vehicle is considered to have a "serious loss of control" by the vehicle being more than 30 degrees off from the target roll and pitch angles continuously for 1 second
// should be called at 10hz
void parachute_check()
{
static uint8_t control_loss_count; // number of iterations we have been out of control
static int32_t baro_alt_start;
// exit immediately if parachute is not enabled
if (!parachute.enabled()) {
return;
}
// call update to give parachute a chance to move servo or relay back to off position
parachute.update();
// return immediately if motors are not armed or pilot's throttle is above zero
if (!motors.armed()) {
control_loss_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) {
control_loss_count = 0;
return;
}
2014-04-03 05:55:34 -03:00
// ensure we are flying
if (ap.land_complete) {
control_loss_count = 0;
return;
}
2014-04-03 05:55:34 -03:00
// ensure the first control_loss event is from above the min altitude
if (control_loss_count == 0 && parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100)) {
2014-04-03 05:55:34 -03:00
return;
}
// get desired lean angles
const Vector3f& target_angle = attitude_control.angle_ef_targets();
// check roll and pitch angles
if (labs(ahrs.roll_sensor - target_angle.x) > CRASH_CHECK_ANGLE_DEVIATION_CD ||
labs(ahrs.pitch_sensor - target_angle.y) > CRASH_CHECK_ANGLE_DEVIATION_CD) {
control_loss_count++;
// don't let control_loss_count get too high
if (control_loss_count > PARACHUTE_CHECK_ITERATIONS_MAX) {
control_loss_count = PARACHUTE_CHECK_ITERATIONS_MAX;
}
2014-04-03 05:55:34 -03:00
// record baro alt if we have just started losing control
if (control_loss_count == 1) {
baro_alt_start = baro_alt;
// exit if baro altitude change indicates we are not falling
}else if (baro_alt >= baro_alt_start) {
control_loss_count = 0;
return;
// To-Do: add check that the vehicle is actually falling
// check if loss of control for at least 1 second
}else if (control_loss_count >= PARACHUTE_CHECK_ITERATIONS_MAX) {
// reset control loss counter
control_loss_count = 0;
2014-04-03 05:55:34 -03:00
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_LOSS_OF_CONTROL);
// release parachute
parachute_release();
}
}else{
// we are not inverted so reset counter
control_loss_count = 0;
}
}
// parachute_release - trigger the release of the parachute, disarm the motors and notify the user
static void parachute_release()
{
2014-04-03 05:55:34 -03:00
// send message to gcs and dataflash
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Released!"));
2014-04-03 05:55:34 -03:00
Log_Write_Event(DATA_PARACHUTE_RELEASED);
// disarm motors
init_disarm_motors();
// release parachute
parachute.release();
}
// parachute_manual_release - trigger the release of the parachute, after performing some checks for pilot error
// checks if the vehicle is landed
static void parachute_manual_release()
{
// exit immediately if parachute is not enabled
if (!parachute.enabled()) {
return;
}
// do not release if vehicle is landed
// do not release if we are landed or below the minimum altitude above home
if (ap.land_complete) {
// warn user of reason for failure
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Landed"));
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_LANDED);
return;
}
// do not release if we are landed or below the minimum altitude above home
if ((parachute.alt_min() != 0 && (current_loc.alt < (int32_t)parachute.alt_min() * 100))) {
// warn user of reason for failure
2014-04-03 05:55:34 -03:00
gcs_send_text_P(SEVERITY_HIGH,PSTR("Parachute: Too Low"));
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_PARACHUTE, ERROR_CODE_PARACHUTE_TOO_LOW);
return;
}
// if we get this far release parachute
parachute_release();
}
2014-04-03 05:55:34 -03:00
#endif // PARACHUTE == ENABLED