Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Remove ekf_check.cpp and unused ekf failsafe methods

This commit is contained in:
Jacob Walser 2017-04-08 09:29:34 -04:00
parent 3852427e0b
commit 0b1a2c3959
3 changed files with 0 additions and 173 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/ArduSub.cpp
View File

@ -39,7 +39,6 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
SCHED_TASK(barometer_accumulate, 50, 90),
SCHED_TASK(update_notify, 50, 90),
SCHED_TASK(one_hz_loop, 1, 100),
SCHED_TASK(ekf_check, 10, 75),
SCHED_TASK(gcs_check_input, 400, 180),
SCHED_TASK(gcs_send_heartbeat, 1, 110),
SCHED_TASK(gcs_send_deferred, 50, 550),

4
ArduSub/Sub.h
View File

@ -591,10 +591,6 @@ private:
bool manual_init(bool ignore_checks);
void manual_run();
void failsafe_crash_check();
void ekf_check();
bool ekf_over_threshold();
void failsafe_ekf_event();
void failsafe_ekf_off_event(void);
void failsafe_battery_check(void);
void failsafe_gcs_check();
void failsafe_manual_control_check(void);

168
ArduSub/ekf_check.cpp
View File

@ -1,168 +0,0 @@
#include "Sub.h"
/**
*
* ekf_check.pde - detects failures of the ekf or inertial nav system
* triggers an alert to the pilot and helps take countermeasures
*
*/
#ifndef EKF_CHECK_ITERATIONS_MAX
# define EKF_CHECK_ITERATIONS_MAX 10 // 1 second (ie. 10 iterations at 10hz) of bad variances signals a failure
#endif
#ifndef EKF_CHECK_WARNING_TIME
# define EKF_CHECK_WARNING_TIME (30*1000) // warning text messages are sent to ground no more than every 30 seconds
#endif
////////////////////////////////////////////////////////////////////////////////
// EKF_check strucutre
////////////////////////////////////////////////////////////////////////////////
static struct {
uint8_t fail_count; // number of iterations ekf or dcm have been out of tolerances
uint8_t bad_variance : 1; // true if ekf should be considered untrusted (fail_count has exceeded EKF_CHECK_ITERATIONS_MAX)
uint32_t last_warn_time; // system time of last warning in milliseconds. Used to throttle text warnings sent to GCS
} ekf_check_state;
// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe
// should be called at 10hz
void Sub::ekf_check()
{
// exit immediately if ekf has no origin yet - this assumes the origin can never become unset
Location temp_loc;
if (!ahrs.get_origin(temp_loc)) {
return;
}
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
if (!motors.armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
ekf_check_state.fail_count = 0;
ekf_check_state.bad_variance = false;
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
failsafe_ekf_off_event(); // clear failsafe
return;
}
// compare compass and velocity variance vs threshold
if (ekf_over_threshold()) {
// if compass is not yet flagged as bad
if (!ekf_check_state.bad_variance) {
// increase counter
ekf_check_state.fail_count++;
// if counter above max then trigger failsafe
if (ekf_check_state.fail_count >= EKF_CHECK_ITERATIONS_MAX) {
// limit count from climbing too high
ekf_check_state.fail_count = EKF_CHECK_ITERATIONS_MAX;
ekf_check_state.bad_variance = true;
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_BAD_VARIANCE);
// send message to gcs
if ((AP_HAL::millis() - ekf_check_state.last_warn_time) > EKF_CHECK_WARNING_TIME) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"EKF variance");
ekf_check_state.last_warn_time = AP_HAL::millis();
}
failsafe_ekf_event();
}
}
} else {
// reduce counter
if (ekf_check_state.fail_count > 0) {
ekf_check_state.fail_count--;
// if compass is flagged as bad and the counter reaches zero then clear flag
if (ekf_check_state.bad_variance && ekf_check_state.fail_count == 0) {
ekf_check_state.bad_variance = false;
// log recovery in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_EKFCHECK, ERROR_CODE_EKFCHECK_VARIANCE_CLEARED);
// clear failsafe
failsafe_ekf_off_event();
}
}
}
// set AP_Notify flags
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
// To-Do: add ekf variances to extended status
}
// ekf_over_threshold - returns true if the ekf's variance are over the tolerance
bool Sub::ekf_over_threshold()
{
// return false immediately if disabled
if (g.fs_ekf_thresh <= 0.0f) {
return false;
}
// return true immediately if position is bad
if (!ekf_position_ok() && !optflow_position_ok()) {
return true;
}
// use EKF to get variance
float posVar, hgtVar, tasVar;
Vector3f magVar;
Vector2f offset;
float compass_variance;
float vel_variance;
ahrs.get_variances(vel_variance, posVar, hgtVar, magVar, tasVar, offset);
compass_variance = magVar.length();
// return true if compass and velocity variance over the threshold
return (compass_variance >= g.fs_ekf_thresh && vel_variance >= g.fs_ekf_thresh);
}
// failsafe_ekf_event - perform ekf failsafe
void Sub::failsafe_ekf_event()
{
// // return immediately if ekf failsafe already triggered
// if (failsafe.ekf) {
// return;
// }
//
// // do nothing if motors disarmed
// if (!motors.armed()) {
// return;
// }
//
// // do nothing if not in GPS flight mode and ekf-action is not land-even-stabilize
// if (!mode_requires_GPS(control_mode) && (g.fs_ekf_action != FS_EKF_ACTION_LAND_EVEN_STABILIZE)) {
// return;
// }
//
// // EKF failsafe event has occurred
// failsafe.ekf = true;
// Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED);
//
// // take action based on fs_ekf_action parameter
// switch (g.fs_ekf_action) {
// case FS_EKF_ACTION_ALTHOLD:
// // AltHold
// if (failsafe.manual_control || !set_mode(ALT_HOLD, MODE_REASON_EKF_FAILSAFE)) {
// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
// }
// break;
// default:
// set_mode_land_with_pause(MODE_REASON_EKF_FAILSAFE);
// break;
// }
//
// // if flight mode is already LAND ensure it's not the GPS controlled LAND
// if (control_mode == LAND) {
// land_do_not_use_GPS();
// }
}
// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared
void Sub::failsafe_ekf_off_event(void)
{
// return immediately if not in ekf failsafe
if (!failsafe.ekf) {
return;
}
// clear flag and log recovery
failsafe.ekf = false;
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_RESOLVED);
}