AP_Airspeed: This adds the ability to turn off and on the airspeed sensor if faulty.

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, AP_AIRSPEED_OPTIONS_DEFAULT),
+
     // @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
@@ -434,6 +446,13 @@ void AP_Airspeed::update(bool log)
         }
     }
 
+    // check for a sensor failure and perform sensor failsafe and recovery if enabled
+    if ((AP_AIRSPEED_OPTIONS_FAILURE_MASK_DISABLE | AP_AIRSPEED_OPTIONS_FAILURE_MASK_REENABLE) & _options) {
+        for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
+            check_sensor_failures(i);
+        }
+    }
+
     // setup primary
     if (healthy(primary_sensor.get())) {
         primary = primary_sensor.get();
@@ -485,5 +504,75 @@ bool AP_Airspeed::all_healthy(void) const
     return true;
 }
 
+
+void AP_Airspeed::check_sensor_failures(uint8_t i)
+{
+    const uint32_t now_ms = AP_HAL::millis();
+    if ((now_ms - state[i].failures.last_check_ms) <= AP_AIRSPEED_OPTIONS_FAILURE_SAMPLE_PERIOD_MS) {
+        // 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
+    if (AP::gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
+        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
+
+    // if "disable" option is allowed and sensor is enabled
+    if (param[i].use > 0 && (AP_AIRSPEED_OPTIONS_FAILURE_MASK_DISABLE & _options)) {
+        // and is probably not healthy
+        if (state[i].failures.health_probability < AP_AIRSPEED_OPTIONS_FAILURE_DISABLE_PROB_THRESH_CRIT) {
+            gcs().send_text(MAV_SEVERITY_WARNING, "Airspeed sensor %d fault - AHRS_WIND_MAX exceeded. Disabled", 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 < AP_AIRSPEED_OPTIONS_FAILURE_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 - AHRS_WIND_MAX exceeded", i+1);
+
+        } else if (state[i].failures.health_probability > AP_AIRSPEED_OPTIONS_FAILURE_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_OPTIONS_FAILURE_MASK_REENABLE & _options) &&
+        state[i].failures.param_use_backup > 0 &&
+        state[i].failures.health_probability > AP_AIRSPEED_OPTIONS_FAILURE_RE_ENABLE_PROB_THRESH_OK) {
+
+        gcs().send_text(MAV_SEVERITY_WARNING, "Airspeed sensor %d fault cleared. 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;
+    }
+}
+
 // singleton instance
 AP_Airspeed *AP_Airspeed::_singleton;

libraries/AP_Airspeed/AP_Airspeed.h

@@ -10,6 +10,15 @@ class AP_Airspeed_Backend;
 
 #define AIRSPEED_MAX_SENSORS 2
 
+// Bitmask for airspeed options
+#define AP_AIRSPEED_OPTIONS_DEFAULT                             0       // No options on by default
+#define AP_AIRSPEED_OPTIONS_FAILURE_MASK_DISABLE                (1<<0)  // If set then use airspeed failure check
+#define AP_AIRSPEED_OPTIONS_FAILURE_MASK_REENABLE               (1<<1)  // If set then automatically enable the airspeed sensor use when healthy again.
+#define AP_AIRSPEED_OPTIONS_FAILURE_SAMPLE_PERIOD_MS            200     // Check the status every 200ms
+#define AP_AIRSPEED_OPTIONS_FAILURE_DISABLE_PROB_THRESH_CRIT    0.1f
+#define AP_AIRSPEED_OPTIONS_FAILURE_DISABLE_PROB_THRESH_WARN    0.5f
+#define AP_AIRSPEED_OPTIONS_FAILURE_RE_ENABLE_PROB_THRESH_OK    0.9f
+
 class Airspeed_Calibration {
 public:
     friend class AP_Airspeed;
@@ -175,6 +184,7 @@ private:
     static AP_Airspeed *_singleton;
 
     AP_Int8 primary_sensor;
+    AP_Int32 _options;    // bitmask options for airspeed
     
     struct {
         AP_Float offset;
@@ -213,6 +223,13 @@ private:
         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
@@ -224,6 +241,7 @@ private:
     float get_pressure(uint8_t i);
     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(uint8_t i);
 
     AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
 };