Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-10 01:44:00 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Implement crash check

Browse Source
This commit is contained in:
Jacob Walser 2017-03-09 12:26:20 -05:00 committed by Randy Mackay
parent 93c6f64a91
commit 5acfc164df
4 changed files with 16 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/ArduSub.cpp
View File

@ -191,6 +191,8 @@ void Sub::fast_loop()
// check if ekf has reset target heading
check_ekf_yaw_reset();
crash_check(MAIN_LOOP_SECONDS);
// run the attitude controllers
update_flight_mode();

1
ArduSub/Sub.cpp
View File

@ -50,7 +50,6 @@ Sub::Sub(void) :
target_rangefinder_alt(0.0f),
baro_alt(0),
baro_climbrate(0.0f),
land_accel_ef_filter(LAND_DETECTOR_ACCEL_LPF_CUTOFF),
rc_throttle_control_in_filter(1.0f),
auto_yaw_mode(AUTO_YAW_LOOK_AT_NEXT_WP),
yaw_look_at_WP_bearing(0.0f),

3
ArduSub/Sub.h
View File

@ -355,7 +355,6 @@ private:
float target_rangefinder_alt; // desired altitude in cm above the ground
int32_t baro_alt; // barometer altitude in cm above home
float baro_climbrate; // barometer climbrate in cm/s
LowPassFilterVector3f land_accel_ef_filter; // accelerations for land and crash detector tests
// Turn counter
int32_t quarter_turn_count;
@ -644,7 +643,7 @@ private:
void stabilize_run();
bool manual_init(bool ignore_checks);
void manual_run();
void crash_check();
void crash_check(uint32_t dt_seconds);
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();

17
ArduSub/crash_check.cpp
View File

@ -4,28 +4,37 @@
#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()
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 stabilize flight mode
if (control_mode == ACRO) {
// 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 (land_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
if (crash_accel_ef_filter.get().length() >= CRASH_CHECK_ACCEL_MAX) {
crash_counter = 0;
return;
}