Plane to use AHRS_WIND_MAX as documented

libraries/AP_AHRS/AP_AHRS.h

@@ -304,6 +304,11 @@ class AP_AHRS
         return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy();
     }
 
+    // return the parameter AHRS_WIND_MAX in metres per second
+    uint8_t get_max_wind() const {
+        return _wind_max;
+    }
+
     // return a ground vector estimate in meters/second, in North/East order
     virtual Vector2f groundspeed_vector(void);
 

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -129,6 +129,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0),
 
+    // @Param: _OPTIONS
+    // @DisplayName: Airspeed options bitmask
+    // @Description: Bitmask of options to use with airspeed.
+    // @Bitmask: 0:Disable on sensor failure,1:Re-enable on sensor recovery
+    // @User: Advanced
+    AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0),
+
     // @Param: 2_TYPE
     // @DisplayName: Second Airspeed type
     // @Description: Type of 2nd airspeed sensor
@@ -194,7 +201,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary)
     // @User: Advanced
     AP_GROUPINFO("2_BUS",  20, AP_Airspeed, param[1].bus, 1),
-
+
+    // Note that 21 is used above by the _OPTIONS parameter.  Do not use 21.
+
     AP_GROUPEND
 };
 
@@ -229,6 +238,9 @@ void AP_Airspeed::init()
         state[i].calibration.init(param[i].ratio);
         state[i].last_saved_ratio = param[i].ratio;
 
+        // Set the enable automatically to false and set the probability that the airspeed is healhy to start with
+        state[i].failures.health_probability = 1.0f;
+
         switch ((enum airspeed_type)param[i].type.get()) {
         case TYPE_NONE:
             // nothing to do
@@ -427,22 +439,24 @@ void AP_Airspeed::update(bool log)
     }
 #endif
 
-    if (log) {
-        AP_Logger *_dataflash = AP_Logger::instance();
-        if (_dataflash != nullptr) {
-            _dataflash->Write_Airspeed(*this);
-        }
-    }
-
     // setup primary
     if (healthy(primary_sensor.get())) {
         primary = primary_sensor.get();
-        return;
+    } else {
+        for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+            if (healthy(i)) {
+                primary = i;
+                break;
+            }
+        }
     }
-    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
-        if (healthy(i)) {
-            primary = i;
-            break;
+
+    check_sensor_failures();
+
+    if (log) {
+        AP_Logger *_dataflash = AP_Logger::instance();
+        if (_dataflash != nullptr) {
+            _dataflash->Write_Airspeed(*this);
         }
     }
 }

libraries/AP_Airspeed/AP_Airspeed.h

@@ -125,6 +125,12 @@ class AP_Airspeed
     }
     float get_EAS2TAS(void) const { return get_EAS2TAS(primary); }
 
+    // get the failure health probability
+    float get_health_failure_probability(uint8_t i) const {
+        return state[i].failures.health_probability;
+    }
+    float get_health_failure_probability(void) const { return get_health_failure_probability(primary); }
+
     // update airspeed ratio calibration
     void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
 
@@ -154,6 +160,11 @@ class AP_Airspeed
                             PITOT_TUBE_ORDER_NEGATIVE = 1,
                             PITOT_TUBE_ORDER_AUTO     = 2 };
 
+    enum OptionsMask {
+        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.
+    };
+
     enum airspeed_type {
         TYPE_NONE=0,
         TYPE_I2C_MS4525=1,
@@ -175,6 +186,7 @@ class AP_Airspeed
     static AP_Airspeed *_singleton;
 
     AP_Int8 primary_sensor;
+    AP_Int32 _options;    // bitmask options for airspeed
 
     struct {
         AP_Float offset;
@@ -213,6 +225,13 @@ class AP_Airspeed
         Airspeed_Calibration calibration;
         float last_saved_ratio;
         uint8_t counter;
+
+        struct {
+            uint32_t last_check_ms;
+            float health_probability;
+            int8_t param_use_backup;
+            bool has_warned;
+        } failures;
     } state[AIRSPEED_MAX_SENSORS];
 
     // current primary sensor
@@ -225,5 +244,8 @@ class AP_Airspeed
     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 check_sensor_failures();
+    void check_sensor_ahrs_wind_max_failures(uint8_t i);
+
     AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
 };

libraries/AP_Airspeed/AP_Airspeed_Health.cpp

@@ -0,0 +1,86 @@
+#include <AP_Common/AP_Common.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_Math/AP_Math.h>
+#include "AP_Airspeed.h"
+
+void AP_Airspeed::check_sensor_failures()
+{
+    for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+        check_sensor_ahrs_wind_max_failures(i);
+    }
+}
+
+void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    if ((now_ms - state[i].failures.last_check_ms) <= 200) {
+        // slow the checking rate
+        return;
+    }
+
+    const float aspeed = get_airspeed();
+    const float wind_max = AP::ahrs().get_max_wind();
+
+    if (aspeed <= 0 || wind_max <= 0) {
+        // invalid estimates
+        return;
+    }
+
+    state[i].failures.last_check_ms = now_ms;
+
+    // update state[i].failures.health_probability via LowPassFilter
+    float speed_accuracy;
+    if (AP::gps().speed_accuracy(speed_accuracy)) {
+        const float gnd_speed = AP::gps().ground_speed();
+
+        if (aspeed > (gnd_speed + wind_max) || aspeed < (gnd_speed - wind_max)) {
+            // bad, decay fast
+            const float probability_coeff = 0.90f;
+            //state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability + (1.0f-probability_coeff)*0.0f;
+            state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability; // equivalent
+
+        } else if (aspeed < (gnd_speed + wind_max) && aspeed > (gnd_speed - wind_max)) {
+            // 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;
+        }
+    }
+
+
+    // Now check if we need to disable or enable the sensor
+
+    // here are some probability thresholds
+    static const float DISABLE_PROB_THRESH_CRIT = 0.1f;
+    static const float DISABLE_PROB_THRESH_WARN = 0.5f;
+    static const float RE_ENABLE_PROB_THRESH_OK = 0.95f;
+
+    // if "disable" option is allowed and sensor is enabled
+    if (param[i].use > 0 && (AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options)) {
+        // and is probably not healthy
+        if (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT) {
+            gcs().send_text(MAV_SEVERITY_ERROR, "Airspeed sensor %d failure. Disabling", i+1);
+            state[i].failures.param_use_backup = param[i].use;
+            param[i].use.set_and_notify(0);
+            state[i].healthy = false;
+
+        // and is probably getting close to not healthy
+        } else if ((state[i].failures.health_probability < DISABLE_PROB_THRESH_WARN) && !state[i].failures.has_warned) {
+            state[i].failures.has_warned = true;
+            gcs().send_text(MAV_SEVERITY_WARNING, "Airspeed sensor %d warning", i+1);
+
+        } else if (state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK) {
+            state[i].failures.has_warned = false;
+        }
+
+    // if Re-Enable options is allowed, and sensor is disabled but was previously enabled, and is probably healthy
+    } else if (param[i].use == 0 &&
+        (AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & _options) &&
+        state[i].failures.param_use_backup > 0 &&
+        state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK) {
+
+        gcs().send_text(MAV_SEVERITY_NOTICE, "Airspeed sensor %d now OK. Re-enabled", i+1);
+        param[i].use.set_and_notify(state[i].failures.param_use_backup); // resume
+        state[i].failures.param_use_backup = -1; // set to invalid so we don't use it
+        state[i].failures.has_warned = false;
+    }
+}

libraries/AP_Logger/LogFile.cpp

@@ -1533,6 +1533,7 @@ void AP_Logger::Write_Airspeed(AP_Airspeed &airspeed)
             offset        : airspeed.get_offset(i),
             use           : airspeed.use(i),
             healthy       : airspeed.healthy(i),
+            health_prob   : airspeed.get_health_failure_probability(i),
             primary       : airspeed.get_primary()
         };
         WriteBlock(&pkt, sizeof(pkt));

libraries/AP_Logger/LogStructure.h

@@ -858,6 +858,7 @@ struct PACKED log_AIRSPEED {
     float   offset;
     bool    use;
     bool    healthy;
+    float   health_prob;
     uint8_t primary;
 };
 
@@ -1157,10 +1158,10 @@ struct PACKED log_DSTL {
 #define CURR_CELL_UNITS  "svvvvvvvvvvv"
 #define CURR_CELL_MULTS  "F00000000000"
 
-#define ARSP_LABELS "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset,U,Health,Primary"
-#define ARSP_FMT "QffcffBBB"
-#define ARSP_UNITS "snPOPP---"
-#define ARSP_MULTS "F00B00---"
+#define ARSP_LABELS "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset,U,Health,Hfp,Pri"
+#define ARSP_FMT "QffcffBBfB"
+#define ARSP_UNITS "snPOPP----"
+#define ARSP_MULTS "F00B00----"
 
 /*
 Format characters in the format string for binary log messages