mirror of https://github.com/ArduPilot/ardupilot
62 lines
2.3 KiB
C++
62 lines
2.3 KiB
C++
#include "Sub.h"
|
|
|
|
// Code to detect a crash
|
|
#define CRASH_CHECK_TRIGGER_SEC 2 // 2 seconds inverted indicates a crash
|
|
#define CRASH_CHECK_ANGLE_DEVIATION_DEG 30.0f // 30 degrees beyond angle max is signal we are inverted
|
|
#define CRASH_CHECK_ACCEL_MAX 3.0f // vehicle must be accelerating less than 3m/s/s to be considered crashed
|
|
#define CRASH_CHECK_ACCEL_LPF_CUTOFF 1.0f // LPF cutoff frequency for acceleration vector
|
|
|
|
// 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
|
|
// called at MAIN_LOOP_RATE
|
|
void Sub::crash_check(uint32_t dt_seconds)
|
|
{
|
|
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
|
|
|
|
static LowPassFilterVector3f crash_accel_ef_filter(CRASH_CHECK_ACCEL_LPF_CUTOFF); // acceleration for checking if crashed
|
|
|
|
// update 1hz filtered acceleration
|
|
Vector3f accel_ef = ahrs.get_accel_ef_blended();
|
|
accel_ef.z += GRAVITY_MSS;
|
|
|
|
crash_accel_ef_filter.apply(accel_ef, dt_seconds);
|
|
|
|
// return immediately if disarmed, or crash checking disabled
|
|
if (!motors.armed() || g.fs_crash_check == 0) {
|
|
crash_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// return immediately if we are not in an angle stabilized flight mode
|
|
if (control_mode == ACRO || control_mode == MANUAL) {
|
|
crash_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// vehicle not crashed if 1hz filtered acceleration is more than 3m/s (1G on Z-axis has been subtracted)
|
|
if (crash_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
|
|
crash_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// check for angle error over 30 degrees
|
|
const float angle_error = attitude_control.get_att_error_angle_deg();
|
|
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
|
|
crash_counter = 0;
|
|
return;
|
|
}
|
|
|
|
// we may be crashing
|
|
crash_counter++;
|
|
|
|
// check if crashing for 2 seconds
|
|
if (crash_counter >= (CRASH_CHECK_TRIGGER_SEC * scheduler.get_loop_rate_hz())) {
|
|
// 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(MAV_SEVERITY_EMERGENCY,"Crash: Disarming");
|
|
// disarm motors
|
|
init_disarm_motors();
|
|
}
|
|
}
|