AP_Airspeed: add dedicated MAX_WIND param

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -69,6 +69,8 @@ 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
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
 
@@ -158,10 +160,24 @@ 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.
+    // @Description: Bitmask of options to use with airspeed. Disable and/or re-enable sensor based on the difference between airspeed and ground speed based on ARSPD_WIND_MAX threshold, if set
-    // @Bitmask: 0:Disable on sensor failure,1:Re-enable on sensor recovery
+    // @Bitmask: 0:Disable sensor, 1:Re-enable sensor
     // @User: Advanced
-    AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0),
+    AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, OPTIONS_DEFAULT),
+
+    // @Param: _WIND_MAX
+    // @DisplayName: Maximum airspeed and ground speed difference
+    // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will be marked unhealthy. Using ARSPD_OPTION this health value can be used to disable the sensor.
+    // @Units: m/s
+    // @User: Advanced
+    AP_GROUPINFO("_WIND_MAX", 22, AP_Airspeed, _wind_max, 0),
+
+    // @Param: _WIND_WARN
+    // @DisplayName: Airspeed and ground speed difference that gives a warning
+    // @Description: If the difference between airspeed and ground speed is greater than this value the sensor will issue a warning. If 0 ARSPD_WIND_MAX is used.
+    // @Units: m/s
+    // @User: Advanced
+    AP_GROUPINFO("_WIND_WARN", 23, AP_Airspeed, _wind_warn, 0),
 #endif
 
 #if AIRSPEED_MAX_SENSORS > 1
@@ -232,7 +248,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     AP_GROUPINFO("2_BUS",  20, AP_Airspeed, param[1].bus, 1),
 #endif // AIRSPEED_MAX_SENSORS
 
-    // Note that 21 is used above by the _OPTIONS parameter.  Do not use 21.
+    // Note that 21, 22 and 23 are used above by the _OPTIONS, _WIND_MAX and _WIND_WARN parameters.  Do not use them!!
 
     AP_GROUPEND
 };
@@ -265,6 +281,19 @@ void AP_Airspeed::init()
         param[0].type.set_default(TYPE_ANALOG);
     }
 
+#ifndef HAL_BUILD_AP_PERIPH
+    // Switch to dedicated WIND_MAX param
+    const float ahrs_max_wind = AP::ahrs().get_max_wind();
+    if (!_wind_max.configured() && is_positive(ahrs_max_wind)) {
+        _wind_max.set_and_save(ahrs_max_wind);
+
+        // Turn off _options to override the new default
+        if (!_options.configured()) {
+            _options.set_and_save(0);
+        }
+    }
+#endif
+
     for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
 #if AP_AIRSPEED_AUTOCAL_ENABLE
         state[i].calibration.init(param[i].ratio);

libraries/AP_Airspeed/AP_Airspeed.h

@@ -179,7 +179,9 @@ private:
 
     AP_Int8 primary_sensor;
     AP_Int32 _options;    // bitmask options for airspeed
-    
+    AP_Float _wind_max;
+    AP_Float _wind_warn;
+
     struct {
         AP_Float offset;
         AP_Float ratio;
@@ -223,7 +225,7 @@ private:
             uint32_t last_check_ms;
             float health_probability;
             int8_t param_use_backup;
-            bool has_warned;
+            uint32_t last_warn_ms;
         } failures;
     } state[AIRSPEED_MAX_SENSORS];
 

libraries/AP_Airspeed/AP_Airspeed_Health.cpp

@@ -1,6 +1,5 @@
 #include "AP_Airspeed.h"
 
-#include <AP_AHRS/AP_AHRS.h>
 #include <AP_Common/AP_Common.h>
 #include <AP_GPS/AP_GPS.h>
 #include <AP_Math/AP_Math.h>
@@ -22,71 +21,79 @@ void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
         // slow the checking rate
         return;
     }
+    state[i].failures.last_check_ms = now_ms;
 
-    const float aspeed = get_airspeed();
+    if (!is_positive(_wind_max)) {
-    const float wind_max = AP::ahrs().get_max_wind();
+        // nothing to do
-
-    if (aspeed <= 0 || wind_max <= 0) {
-        // invalid estimates
         return;
     }
 
-    state[i].failures.last_check_ms = now_ms;
+    const float aspeed = get_airspeed();
-
+    if (aspeed <= 0) {
-    // update state[i].failures.health_probability via LowPassFilter
+        // invalid estimate
-    float speed_accuracy;
+        return;
-    const AP_GPS &gps = AP::gps();
-    if (gps.speed_accuracy(speed_accuracy)) {
-        const float gnd_speed = 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;
-        }
     }
 
+    const AP_GPS &gps = AP::gps();
+    if (gps.status() < AP_GPS::GPS_Status::GPS_OK_FIX_3D) {
+        // GPS speed can't be trusted, re-enable airspeed as a fallback
+        if ((param[i].use == 0) && (state[i].failures.param_use_backup == 1)) {
+            GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Airspeed sensor %d, Re-enabled as GPS fall-back", i+1);
+            param[i].use.set_and_notify(state[i].failures.param_use_backup); 
+            state[i].failures.param_use_backup = -1;
+        }
+        return;
+    }
+    const float speed_diff = fabsf(aspeed-gps.ground_speed());
+
+    // update health_probability with LowPassFilter
+    if (speed_diff > _wind_max) {
+        // bad, decay fast
+        const float probability_coeff = 0.90f;
+        state[i].failures.health_probability = probability_coeff*state[i].failures.health_probability;
+
+    } else {
+        // 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) {
-    if (param[i].use > 0 && (AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options)) {
+        if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options) != 0) &&
-        // and is probably not healthy
+                (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT)) {
-        if (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT) {
+            // if "disable" option is allowed and sensor is enabled and is probably not healthy
             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
+        // Warn if necessary
-        } 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) {
+        // set warn to max if not set or larger than max
-            state[i].failures.has_warned = false;
+        float wind_warn = _wind_warn;
+        if (!is_positive(wind_warn) || (wind_warn > _wind_max)) {
+            wind_warn = _wind_max;
+        }
+
+        if ((speed_diff > wind_warn) && ((now_ms - state[i].failures.last_warn_ms) > 15000)) {
+            state[i].failures.last_warn_ms = now_ms;
+            GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "Airspeed %d warning %0.1fm/s air to gnd speed diff", i+1, speed_diff);
         }
 
     // if Re-Enable options is allowed, and sensor is disabled but was previously enabled, and is probably healthy
-    } else if (param[i].use == 0 &&
+    } else if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & _options) != 0) &&
-        (AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & _options) &&
+                (state[i].failures.param_use_backup > 0) && 
-        state[i].failures.param_use_backup > 0 &&
+                (state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK)) {
-        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;
     }
 }