diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 3b37ea4e0f..3c2007d66b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/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. - // @Bitmask: 0:Disable on sensor failure,1:Re-enable on sensor recovery + // @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 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 #include #include #include @@ -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(); - const float wind_max = AP::ahrs().get_max_wind(); - - if (aspeed <= 0 || wind_max <= 0) { - // invalid estimates + if (!is_positive(_wind_max)) { + // nothing to do return; } - state[i].failures.last_check_ms = now_ms; - - // update state[i].failures.health_probability via LowPassFilter - float speed_accuracy; - 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 float aspeed = get_airspeed(); + if (aspeed <= 0) { + // invalid estimate + return; } + 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 && (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) { + if (param[i].use > 0) { + if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options) != 0) && + (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 - } 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); + // Warn if necessary - } else if (state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK) { - state[i].failures.has_warned = false; + // set warn to max if not set or larger than max + 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 && - (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) { + } else if (((AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_RECOVERY_DO_REENABLE & _options) != 0) && + (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; } }