diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 1bbbf85b5c..4f48807921 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/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)); diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index b1871b42de..befefdbdc1 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/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); diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index 432cc801ac..464e752f3b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include 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;