AP_Airspeed: Enable use of EKF3 to check airspeed health

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -87,7 +87,7 @@ extern const AP_HAL::HAL &hal;
 #define PSI_RANGE_DEFAULT 1.0f
 #endif
 
-#define OPTIONS_DEFAULT AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE | AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE
+#define OPTIONS_DEFAULT AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE | AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE | AP_Airspeed::OptionsMask::USE_EKF_CONSISTENCY
 
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
@@ -188,9 +188,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
 #ifndef HAL_BUILD_AP_PERIPH
     // @Param: _OPTIONS
     // @DisplayName: Airspeed options bitmask
-    // @Description: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction
+    // @Description: Bitmask of options to use with airspeed. 0:Disable use based on airspeed/groundspeed mismatch (see ARSPD_WIND_MAX), 1:Automatically reenable use based on airspeed/groundspeed mismatch recovery (see ARSPD_WIND_MAX) 2:Disable voltage correction, 3:Check that the airspeed is statistically consistent with the navigation EKF vehicle and wind velocity estimates using EKF3 (requires AHRS_EKF_TYPE = 3)
     // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle. Always set to 0.
-    // @Bitmask: 0:SpeedMismatchDisable, 1:AllowSpeedMismatchRecovery, 2:DisableVoltageCorrection
+    // @Bitmask: 0:SpeedMismatchDisable, 1:AllowSpeedMismatchRecovery, 2:DisableVoltageCorrection, 3:UseEkf3Consistency
     // @User: Advanced
     AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, OPTIONS_DEFAULT),
 
@@ -209,6 +209,16 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Units: m/s
     // @User: Advanced
     AP_GROUPINFO("_WIND_WARN", 23, AP_Airspeed, _wind_warn, 0),
+
+    // @Param: _WIND_GATE
+    // @DisplayName: Re-enable Consistency Check Gate Size
+    // @Description: Number of standard deviations applied to the re-enable EKF consistency check that is used when ARSPD_OPTIONS bit position 3 is set. Larger values will make the re-enabling of the airspeed sensor faster, but increase the likelihood of re-enabling a degraded sensor.
+    // @Description{Copter, Blimp, Rover, Sub}: This parameter and function is not used by this vehicle.
+    // @Units: StdDev
+    // @Range: 0.0 10.0
+    // @User: Advanced
+    AP_GROUPINFO("_WIND_GATE", 26, AP_Airspeed, _wind_gate, 5.0f),
+
 #endif
 
 #if AIRSPEED_MAX_SENSORS > 1
@@ -292,7 +302,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     
 #endif // AIRSPEED_MAX_SENSORS
 
-    // Note that 21, 22 and 23 are used above by the _OPTIONS, _WIND_MAX and _WIND_WARN parameters.  Do not use them!!
+    // Note that 21, 22, 23 and 26 are used above by the _OPTIONS, _WIND_MAX, _WIND_WARN and _WIND_GATE parameters.  Do not use them!!
 
     // NOTE: Index 63 is used by AIRSPEED_TYPE, Do not use it!: AP_Param converts an index of 0 to 63 so that the index may be bit shifted
     AP_GROUPEND
@@ -713,6 +723,7 @@ void AP_Airspeed::Log_Airspeed()
             use           : use(i),
             healthy       : healthy(i),
             health_prob   : get_health_probability(i),
+            test_ratio    : get_test_ratio(i),
             primary       : get_primary()
         };
         AP::logger().WriteBlock(&pkt, sizeof(pkt));

libraries/AP_Airspeed/AP_Airspeed.h

@@ -136,6 +136,7 @@ public:
         ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE                   = (1<<0),   // If set then use airspeed failure check
         ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE         = (1<<1),   // If set then automatically enable the airspeed sensor use when healthy again.
         DISABLE_VOLTAGE_CORRECTION                            = (1<<2),
+        USE_EKF_CONSISTENCY                                   = (1<<3),
     };
 
     enum airspeed_type {
@@ -183,6 +184,7 @@ private:
     AP_Int32 _options;    // bitmask options for airspeed
     AP_Float _wind_max;
     AP_Float _wind_warn;
+    AP_Float _wind_gate;
 
     struct {
         AP_Float offset;
@@ -225,6 +227,7 @@ private:
         struct {
             uint32_t last_check_ms;
             float health_probability;
+            float test_ratio;
             int8_t param_use_backup;
             uint32_t last_warn_ms;
         } failures;
@@ -254,6 +257,14 @@ private:
         return get_health_probability(primary);
     }
 
+    // get the consistency test ratio
+    float get_test_ratio(uint8_t i) const {
+        return state[i].failures.test_ratio;
+    }
+    float get_test_ratio(void) const {
+        return get_test_ratio(primary);
+    }
+
     void update_calibration(uint8_t i, float raw_pressure);
     void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
     void send_airspeed_calibration(const Vector3f &vg);

libraries/AP_Airspeed/AP_Airspeed_Health.cpp

@@ -4,6 +4,8 @@
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Logger/AP_Logger.h>
 
 void AP_Airspeed::check_sensor_failures()
 {
@@ -43,15 +45,31 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
         }
         return;
     }
-    const float speed_diff = fabsf(state[i].airspeed-gps.ground_speed());
 
+    // check for airspeed consistent with wind and vehicle velocity using the EKF
+    uint32_t age_ms;
+    float innovation, innovationVariance;
+    if (AP::ahrs().airspeed_health_data(innovation, innovationVariance, age_ms) && age_ms < 1000 && is_positive(innovationVariance)) {
+        state[i].failures.test_ratio = fabsf(innovation) / safe_sqrt(innovationVariance);
+    } else {
+        state[i].failures.test_ratio = 0.0f;
+    }
+    bool data_is_inconsistent;
+    if ((AP_Airspeed::OptionsMask::USE_EKF_CONSISTENCY & _options) != 0) {
+            data_is_inconsistent = state[i].failures.test_ratio > MAX(_wind_gate, 0.0f);
+    } else {
+        data_is_inconsistent = false;
+    }
+
+    const float speed_diff = fabsf(state[i].airspeed-gps.ground_speed());
+    const bool data_is_implausible = speed_diff > _wind_max;
     // update health_probability with LowPassFilter
-    if (speed_diff > _wind_max) {
+    if (data_is_implausible || data_is_inconsistent) {
         // bad, decay fast
         const float probability_coeff = 0.90f;
         state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability;
 
-    } else {
+    } else if (!data_is_implausible && !data_is_inconsistent) {
         // good, grow slow
         const float probability_coeff = 0.98f;
         state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability + (1.0f-probability_coeff)*1.0f;