Rover: add ekf failsafe

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),

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);
 }

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

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;

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);

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 {

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");
+}