From c9af22357921e3ae355c04d84eadc84e89d1ffc6 Mon Sep 17 00:00:00 2001 From: ChrisBird Date: Sun, 3 Feb 2019 03:55:38 -0800 Subject: [PATCH] AP_Airspeed: Renaming the check method name and readding the overall option check. This will set it up for future options. --- libraries/AP_Airspeed/AP_Airspeed.h | 6 +++--- libraries/AP_Airspeed/AP_Airspeed_Health.cpp | 10 ++++++---- 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index a132382449..b1a0c74980 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -155,8 +155,8 @@ public: PITOT_TUBE_ORDER_AUTO = 2 }; enum OptionsMask { - ON_FAILURE_DO_DISABLE = (1<<0), // If set then use airspeed failure check - ON_FAILURE_RECOVERY_DO_REENABLE = (1<<1), // If set then automatically enable the airspeed sensor use when healthy again. + 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 { @@ -239,7 +239,7 @@ private: void update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); void check_sensor_failures(); - void check_sensor_failures(uint8_t i); + void check_sensor_ahrs_wind_max_failures(uint8_t i); AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS]; }; diff --git a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp index 8cda889795..456b175511 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_Health.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -6,11 +6,13 @@ void AP_Airspeed::check_sensor_failures() { for (uint8_t i=0; i 0 && (AP_Airspeed::OptionsMask::ON_FAILURE_DO_DISABLE & _options)) { + 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); @@ -74,7 +76,7 @@ void AP_Airspeed::check_sensor_failures(uint8_t i) // 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_RECOVERY_DO_REENABLE & _options) && + (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) {