From 40cc5a500604fefe8f4020bb44ed5957b0e9e008 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 5 May 2020 09:12:14 +1000 Subject: [PATCH] ArduPlane: Improve EKF failsafe in VTOL modes Replicates Copter behaviour with a three step process if the EKF sustains a loss of navigation as detected by high GPS innovation test ratios: 1) Attempts a yaw reset using the GSF estimate if available 2) Attempts a lane switch 3) Falls back to a non-position mode --- ArduPlane/ArduPlane.cpp | 1 + ArduPlane/Parameters.cpp | 7 ++ ArduPlane/Parameters.h | 2 + ArduPlane/Plane.h | 8 ++ ArduPlane/config.h | 6 ++ ArduPlane/ekf_check.cpp | 203 +++++++++++++++++++++++++++++++++++++++ ArduPlane/quadplane.cpp | 15 +++ ArduPlane/quadplane.h | 1 + 8 files changed, 243 insertions(+) create mode 100644 ArduPlane/ekf_check.cpp diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index abc5c10543..640ac8eee2 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -51,6 +51,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { #if ADVANCED_FAILSAFE == ENABLED SCHED_TASK(afs_fs_check, 10, 100), #endif + SCHED_TASK(ekf_check, 10, 75), SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_receive, 300, 500), SCHED_TASK_CLASS(GCS, (GCS*)&plane._gcs, update_send, 300, 500), SCHED_TASK_CLASS(AP_ServoRelayEvents, &plane.ServoRelayEvents, update_events, 50, 150), diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 9db85756ac..89e8516c0c 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1271,6 +1271,13 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Advanced AP_GROUPINFO("FWD_BAT_IDX", 25, ParametersG2, fwd_thr_batt_idx, 0), + // @Param: FS_EKF_THRESH + // @DisplayName: EKF failsafe variance threshold + // @Description: Allows setting the maximum acceptable compass and velocity variance used to check navigation health in VTOL modes + // @Values: 0.6:Strict, 0.8:Default, 1.0:Relaxed + // @User: Advanced + AP_GROUPINFO("FS_EKF_THRESH", 26, ParametersG2, fs_ekf_thresh, FS_EKF_THRESHOLD_DEFAULT), + AP_GROUPEND }; diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index eabace77fb..269fd9fa3e 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -575,6 +575,8 @@ public: // EFI Engine Monitor AP_EFI efi; #endif + + AP_Float fs_ekf_thresh; }; extern const AP_Param::Info var_info[]; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1c5a3049d7..f0742aaef0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1036,6 +1036,14 @@ private: static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1, "_failsafe_priorities is missing the sentinel"); + // EKF checks for loss of navigation performed in ekf_check.cpp + // These are specific to VTOL operation + void ekf_check(); + bool ekf_over_threshold(); + void failsafe_ekf_event(); + void failsafe_ekf_off_event(void); + bool ekf_position_ok() const; + public: void failsafe_check(void); bool set_target_location(const Location& target_loc) override; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 0274cc3450..cb35d5f96d 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -308,3 +308,9 @@ #ifndef LANDING_GEAR_ENABLED #define LANDING_GEAR_ENABLED !HAL_MINIMIZE_FEATURES #endif + +////////////////////////////////////////////////////////////////////////////// +// EKF Failsafe +#ifndef FS_EKF_THRESHOLD_DEFAULT + # define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered +#endif diff --git a/ArduPlane/ekf_check.cpp b/ArduPlane/ekf_check.cpp new file mode 100644 index 0000000000..f5ecc64409 --- /dev/null +++ b/ArduPlane/ekf_check.cpp @@ -0,0 +1,203 @@ +#include "Plane.h" + +/** + * + * 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 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 + bool failsafe_on; // true when the loss of navigation failsafe is on +} ekf_check_state; + +// ekf_check - detects if ekf variance are out of tolerance and triggers failsafe +// should be called at 10hz +void Plane::ekf_check() +{ + // ensure EKF_CHECK_ITERATIONS_MAX is at least 7 + static_assert(EKF_CHECK_ITERATIONS_MAX >= 7, "EKF_CHECK_ITERATIONS_MAX must be at least 7"); + + // 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 (!plane.arming.is_armed() || quadplane.in_vtol_posvel_mode() || (g2.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 (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-2)) { + // we are two iterations away from declaring an EKF failsafe, ask the EKF if we can reset + // yaw to resolve the issue + ahrs.request_yaw_reset(); + } + if (ekf_check_state.fail_count == (EKF_CHECK_ITERATIONS_MAX-1)) { + // we are just about to declare a EKF failsafe, ask the EKF if we can + // change lanes to resolve the issue + ahrs.check_lane_switch(); + } + // 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; + AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::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; + AP::logger().Write_Error(LogErrorSubsystem::EKFCHECK, LogErrorCode::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 Plane::ekf_over_threshold() +{ + // return false immediately if disabled + if (g2.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); + + const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z); + + // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold + uint8_t over_thresh_count = 0; + if (mag_max >= g2.fs_ekf_thresh) { + over_thresh_count++; + } + + bool optflow_healthy = false; +#if OPTFLOW == ENABLED + optflow_healthy = optflow.healthy(); +#endif + if (!optflow_healthy && (vel_variance >= (2.0f * g2.fs_ekf_thresh))) { + over_thresh_count += 2; + } else if (vel_variance >= g2.fs_ekf_thresh) { + over_thresh_count++; + } + + if ((position_variance >= g2.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { + return true; + } + + if (ekf_position_ok()) { + return false; + } + return true; +} + + +// failsafe_ekf_event - perform ekf failsafe +void Plane::failsafe_ekf_event() +{ + // return immediately if ekf failsafe already triggered + if (ekf_check_state.failsafe_on) { + return; + } + + // EKF failsafe event has occurred + ekf_check_state.failsafe_on = true; + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_OCCURRED); + + // if not in a VTOL mode requring position, then nothing needs to be done + if (!quadplane.in_vtol_posvel_mode()) { + return; + } + + if (quadplane.in_vtol_auto()) { + // the pilot is not controlling via sticks so switch to QLAND + plane.set_mode(mode_qland, ModeReason::EKF_FAILSAFE); + } else { + // the pilot is controlling via sticks so fallbacl to QHOVER + plane.set_mode(mode_qhover, ModeReason::EKF_FAILSAFE); + } +} + +// failsafe_ekf_off_event - actions to take when EKF failsafe is cleared +void Plane::failsafe_ekf_off_event(void) +{ + // return immediately if not in ekf failsafe + if (!ekf_check_state.failsafe_on) { + return; + } + + ekf_check_state.failsafe_on = false; + AP::logger().Write_Error(LogErrorSubsystem::FAILSAFE_EKFINAV, LogErrorCode::FAILSAFE_RESOLVED); +} + +// ekf_position_ok - returns true if the ekf claims it's horizontal absolute position estimate is ok and home position is set +bool Plane::ekf_position_ok() const +{ + if (!ahrs.have_inertial_nav()) { + // do not allow navigation with dcm position + return false; + } + + // with EKF use filter status and ekf check + nav_filter_status filt_status; + if(ahrs.get_filter_status(filt_status)) { + + } + + // if disarmed we accept a predicted horizontal position + if (!plane.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); + } +} \ No newline at end of file diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index fff644b879..44ea9b36f3 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2189,6 +2189,21 @@ bool QuadPlane::in_vtol_mode(void) const in_vtol_auto()); } +/* + are we in a VTOL mode that needs position and velocity estimates? + */ +bool QuadPlane::in_vtol_posvel_mode(void) const +{ + if (!available()) { + return false; + } + return (plane.control_mode == &plane.mode_qloiter || + plane.control_mode == &plane.mode_qland || + plane.control_mode == &plane.mode_qrtl || + plane.control_mode == &plane.mode_qautotune || + ((plane.control_mode == &plane.mode_guided || plane.control_mode == &plane.mode_avoidADSB) && plane.auto_state.vtol_loiter) || + in_vtol_auto()); +} /* main landing controller. Used for landing and RTL. diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 006ed365cf..35d25e6a4c 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -89,6 +89,7 @@ public: bool verify_vtol_land(void); bool in_vtol_auto(void) const; bool in_vtol_mode(void) const; + bool in_vtol_posvel_mode(void) const; void update_throttle_hover(); // vtol help for is_flying()