Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-06 16:03:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Move crash check logic under failsafe.cpp

Browse Source
This commit is contained in:
Jacob Walser 2017-03-24 15:22:46 -04:00
parent bc5d9b1a19
commit 9d4b24c354
2 changed files with 60 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
ArduSub/crash_check.cpp
View File

@ -1,61 +0,0 @@
#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();
}
}

61
ArduSub/failsafe.cpp
View File

@ -287,6 +287,66 @@ void Sub::failsafe_gcs_check()
} }
} }
// 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();
}
}
// executes terrain failsafe if data is missing for longer than a few seconds // executes terrain failsafe if data is missing for longer than a few seconds
// missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully // missing_data should be set to true if the vehicle failed to navigate because of missing data, false if navigation is proceeding successfully
void Sub::failsafe_terrain_check() void Sub::failsafe_terrain_check()
@ -386,4 +446,3 @@ bool Sub::should_disarm_on_failsafe()
break; break;
} }
} }