diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index c1f18d8492..cda5e12ce7 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -74,6 +74,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { #endif SCHED_TASK(gcs_failsafe_check, 10, 200), SCHED_TASK(fence_check, 10, 200), + SCHED_TASK(ekf_check, 10, 100), SCHED_TASK_CLASS(ModeSmartRTL, &rover.mode_smartrtl, save_position, 3, 200), SCHED_TASK_CLASS(AP_Notify, &rover.notify, update, 50, 300), SCHED_TASK(one_second_loop, 1, 1500), diff --git a/APMrover2/AP_Arming.cpp b/APMrover2/AP_Arming.cpp index 40dfd23641..2a4afaef84 100644 --- a/APMrover2/AP_Arming.cpp +++ b/APMrover2/AP_Arming.cpp @@ -47,6 +47,14 @@ bool AP_Arming_Rover::gps_checks(bool display_failure) return true; } + // check for ekf failsafe + if (rover.failsafe.ekf) { + if (display_failure) { + gcs().send_text(MAV_SEVERITY_CRITICAL, "PreArm: EKF failsafe"); + } + return false; + } + // call parent gps checks return AP_Arming::gps_checks(display_failure); } diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 930820e169..54d1d884a8 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -170,6 +170,20 @@ const AP_Param::Info Rover::var_info[] = { // @User: Standard GSCALAR(fs_crash_check, "FS_CRASH_CHECK", FS_CRASH_DISABLE), + // @Param: FS_EKF_ACTION + // @DisplayName: EKF Failsafe Action + // @Description: Controls the action that will be taken when an EKF failsafe is invoked + // @Values: 0:Disabled,1:Hold + // @User: Advanced + GSCALAR(fs_ekf_action, "FS_EKF_ACTION", 1), + + // @Param: FS_EKF_THRESH + // @DisplayName: EKF failsafe variance threshold + // @Description: Allows setting the maximum acceptable compass and velocity variance + // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed + // @User: Advanced + GSCALAR(fs_ekf_thresh, "FS_EKF_THRESH", 0.8f), + // @Param: RNGFND_TRIGGR_CM // @DisplayName: Object avoidance trigger distance // @Description: The distance from an obstacle in centimeters at which the rangefinder triggers a turn to avoid the obstacle diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index fcdc1dc6e3..71a93100b0 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -142,6 +142,8 @@ public: k_param_fs_throttle_value, k_param_fs_gcs_enabled, k_param_fs_crash_check, + k_param_fs_ekf_action, + k_param_fs_ekf_thresh, // 187 // obstacle control k_param_sonar_enabled = 190, // deprecated, can be removed @@ -249,6 +251,8 @@ public: AP_Int16 fs_throttle_value; AP_Int8 fs_gcs_enabled; AP_Int8 fs_crash_check; + AP_Int8 fs_ekf_action; + AP_Float fs_ekf_thresh; // obstacle avoidance control AP_Int16 rangefinder_trigger_cm; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 94da5f0a4e..125e24ac08 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -257,6 +257,7 @@ private: uint8_t triggered; // bit flags of failsafes that have triggered an action uint32_t last_valid_rc_ms; // system time of most recent RC input from pilot uint32_t last_heartbeat_ms; // system time of most recent heartbeat from ground station + bool ekf; } failsafe; // notification object for LEDs, buzzers etc (parameter set to false disables external leds) @@ -419,6 +420,13 @@ private: void cruise_learn_update(); void cruise_learn_complete(); + // ekf_check.cpp + void ekf_check(); + bool ekf_over_threshold(); + bool ekf_position_ok(); + void failsafe_ekf_event(); + void failsafe_ekf_off_event(void); + // failsafe.cpp void failsafe_trigger(uint8_t failsafe_type, bool on); void handle_battery_failsafe(const char* type_str, const int8_t action); diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 89bb2aec51..4350d0aad4 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -74,8 +74,16 @@ #define ERROR_SUBSYSTEM_FAILSAFE_FENCE 9 #define ERROR_SUBSYSTEM_FLIGHT_MODE 10 #define ERROR_SUBSYSTEM_CRASH_CHECK 12 +#define ERROR_SUBSYSTEM_EKFCHECK 16 +#define ERROR_SUBSYSTEM_FAILSAFE_EKFINAV 17 // subsystem specific error codes -- crash checker #define ERROR_CODE_CRASH_CHECK_CRASH 1 +// EKF check definitions +#define ERROR_CODE_EKFCHECK_BAD_VARIANCE 2 +#define ERROR_CODE_EKFCHECK_VARIANCE_CLEARED 0 +// subsystem specific error codes -- ekf failsafe +#define ERROR_CODE_FAILSAFE_RESOLVED 0 +#define ERROR_CODE_FAILSAFE_OCCURRED 1 // radio failsafe enum (FS_THR_ENABLE parameter) enum fs_thr_enable { @@ -97,6 +105,11 @@ enum fs_crash_action { FS_CRASH_HOLD_AND_DISARM = 2 }; +enum fs_ekf_action { + FS_EKF_DISABLE = 0, + FS_EFK_HOLD = 1 +}; + #define DISTANCE_HOME_MAX 0.5f // Distance max to home location before changing it when disarm enum mode_reason_t { diff --git a/APMrover2/ekf_check.cpp b/APMrover2/ekf_check.cpp new file mode 100644 index 0000000000..88ae1b7ee1 --- /dev/null +++ b/APMrover2/ekf_check.cpp @@ -0,0 +1,185 @@ +#include "Rover.h" + +/** + * + * Detects failures of the ekf and triggers a failsafe + * + */ + +#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 structure +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 Rover::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, or ekf check is disabled + if (!arming.is_armed() || (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 variance 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; +} + +// returns true if the ekf's variance are over the tolerance +bool Rover::ekf_over_threshold() +{ + // return false immediately if disabled + if (g.fs_ekf_thresh <= 0.0f) { + return false; + } + + // use EKF to get variance + float position_variance, vel_variance, height_variance, tas_variance; + Vector3f mag_variance; + Vector2f offset; + ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance, offset); + + // return true if two of compass, velocity and position variances are over the threshold + uint8_t over_thresh_count = 0; + if (mag_variance.length() >= g.fs_ekf_thresh) { + over_thresh_count++; + } + if (vel_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + if (position_variance >= g.fs_ekf_thresh) { + over_thresh_count++; + } + + if (over_thresh_count >= 2) { + return true; + } + + if (ekf_position_ok()) { + return false; + } + + return true; +} + +// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set +bool Rover::ekf_position_ok() +{ + if (!ahrs.have_inertial_nav()) { + // do not allow navigation with dcm position + return false; + } + + // get EKF filter status + nav_filter_status filt_status; + rover.ahrs.get_filter_status(filt_status); + + // if disarmed we accept a predicted horizontal position + if (!arming.is_armed()) { + return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs)); + } else { + // once armed we require a good absolute position and EKF must not be in const_pos_mode + return (filt_status.flags.horiz_pos_abs && !filt_status.flags.const_pos_mode); + } +} + +// perform ekf failsafe +void Rover::failsafe_ekf_event() +{ + // return immediately if ekf failsafe already triggered + if (failsafe.ekf) { + return; + } + + // EKF failsafe event has occurred + failsafe.ekf = true; + Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_EKFINAV, ERROR_CODE_FAILSAFE_OCCURRED); + gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF failsafe!"); + + // does this mode require position? + if (!control_mode->requires_position()) { + return; + } + + // take action based on fs_ekf_action parameter + switch ((enum fs_ekf_action)g.fs_ekf_action.get()) { + case FS_EKF_DISABLE: + // do nothing + break; + case FS_EFK_HOLD: + default: + set_mode(mode_hold, MODE_REASON_EKF_FAILSAFE); + break; + } +} + +// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared +void Rover::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); + gcs().send_text(MAV_SEVERITY_CRITICAL,"EKF failsafe cleared"); +}