From 1a04a540f5a4f4682041311569569ae54a962986 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 29 Jan 2019 18:02:40 -0800 Subject: [PATCH] AP_Airspeed: convert the defines into static consts - also perform logging at end of update - convert Options Mask into an enum --- libraries/AP_Airspeed/AP_Airspeed.cpp | 105 +++---------------- libraries/AP_Airspeed/AP_Airspeed.h | 16 ++- libraries/AP_Airspeed/AP_Airspeed_Health.cpp | 86 +++++++++++++++ 3 files changed, 108 insertions(+), 99 deletions(-) create mode 100644 libraries/AP_Airspeed/AP_Airspeed_Health.cpp diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 16f461d94a..e17f729034 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -134,7 +134,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @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), + AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, 0), // @Param: 2_TYPE // @DisplayName: Second Airspeed type @@ -439,31 +439,26 @@ void AP_Airspeed::update(bool log) } #endif + // setup primary + if (healthy(primary_sensor.get())) { + primary = primary_sensor.get(); + } else { + for (uint8_t i=0; iWrite_Airspeed(*this); } } - - // 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= 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; diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index ccf1da71dd..a132382449 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -10,15 +10,6 @@ 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; @@ -163,6 +154,11 @@ public: PITOT_TUBE_ORDER_NEGATIVE = 1, 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. + }; + enum airspeed_type { TYPE_NONE=0, TYPE_I2C_MS4525=1, @@ -241,6 +237,8 @@ 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(); void check_sensor_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 new file mode 100644 index 0000000000..8cda889795 --- /dev/null +++ b/libraries/AP_Airspeed/AP_Airspeed_Health.cpp @@ -0,0 +1,86 @@ +#include +#include +#include +#include "AP_Airspeed.h" + +void AP_Airspeed::check_sensor_failures() +{ + for (uint8_t i=0; i (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 + + // 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_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); + 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); + + } else if (state[i].failures.health_probability > 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::OptionsMask::ON_FAILURE_RECOVERY_DO_REENABLE & _options) && + 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; + } +}