Sub: Implement crash failsafe check/action

This commit is contained in:
Jacob Walser 2017-03-24 16:30:28 -04:00
parent 9d4b24c354
commit b32b552d1e
5 changed files with 47 additions and 41 deletions

View File

@ -185,8 +185,6 @@ 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();
@ -216,6 +214,8 @@ void Sub::fifty_hz_loop()
// check pilot input failsafe
failsafe_manual_control_check();
failsafe_crash_check();
// Update servo output
RC_Channels::set_pwm_all();
SRV_Channels::limit_slew_rate(SRV_Channel::k_mount_tilt, g.cam_slew_limit, 0.02f);

View File

@ -306,9 +306,9 @@ const AP_Param::Info Sub::var_info[] = {
// @Param: FS_CRASH_CHECK
// @DisplayName: Crash check enable
// @Description: This enables automatic crash checking. When enabled the motors will disarm if a crash is detected.
// @Values: 0:Disabled
// @Values: 0:Disabled,1:Warn only,2:Disarm
// @User: Advanced
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", 0),
GSCALAR(fs_crash_check, "FS_CRASH_CHECK", FS_CRASH_DISABLED),
// @Param: JS_GAIN_DEFAULT
// @DisplayName: Default gain at boot

View File

@ -279,6 +279,7 @@ private:
uint8_t leak : 1; // true if leak recently detected
uint8_t internal_pressure : 1; // true if internal pressure is over threshold
uint8_t internal_temperature : 1; // true if temperature is over threshold
uint8_t crash : 1; // true if we are crashed
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
@ -287,6 +288,7 @@ private:
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs
uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs
} failsafe;
// sensor health for logging
@ -618,7 +620,7 @@ private:
void stabilize_run();
bool manual_init(bool ignore_checks);
void manual_run();
void crash_check(uint32_t dt_seconds);
void failsafe_crash_check();
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();

View File

@ -304,6 +304,11 @@ enum RTLState {
#define FS_TEMP_DISABLED 0
#define FS_TEMP_WARN_ONLY 1
// Crash failsafe options
#define FS_CRASH_DISABLED 0
#define FS_CRASH_WARN_ONLY 1
#define FS_CRASH_DISARM 2
// Terrain failsafe actions for AUTO mode
#define FS_TERRAIN_DISARM 0
#define FS_TERRAIN_HOLD 1

View File

@ -287,62 +287,61 @@ 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_TRIGGER_MS 2000 // 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)
// Check for a crash
// The vehicle is considered crashed if the angle error exceeds a specified limit for more than 2 seconds
void Sub::failsafe_crash_check()
{
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);
static uint32_t last_crash_check_pass_ms;
uint32_t tnow = AP_HAL::millis();
// return immediately if disarmed, or crash checking disabled
if (!motors.armed() || g.fs_crash_check == 0) {
crash_counter = 0;
if (!motors.armed() || g.fs_crash_check == FS_CRASH_DISABLED) {
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
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;
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
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;
last_crash_check_pass_ms = tnow;
failsafe.crash = false;
return;
}
// we may be crashing
crash_counter++;
if (tnow < last_crash_check_pass_ms + CRASH_CHECK_TRIGGER_MS) {
return;
}
// 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
// Conditions met, we are in failsafe
// Send warning to GCS
if (tnow > failsafe.last_crash_warn_ms + 20000) {
failsafe.last_crash_warn_ms = tnow;
gcs_send_text(MAV_SEVERITY_WARNING,"Crash detected");
}
// Only perform failsafe action once
if (failsafe.crash) {
return;
}
failsafe.crash = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_CRASH_CHECK, ERROR_CODE_CRASH_CHECK_CRASH);
// disarm motors
if (g.fs_crash_check == FS_CRASH_DISARM) {
init_disarm_motors();
}
}