diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 0690641656..16f461d94a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -129,6 +129,13 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @User: Advanced AP_GROUPINFO("_PRIMARY", 10, AP_Airspeed, primary_sensor, 0), + // @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 + // @User: Advanced + AP_GROUPINFO("_OPTIONS", 21, AP_Airspeed, _options, AP_AIRSPEED_OPTIONS_DEFAULT), + // @Param: 2_TYPE // @DisplayName: Second Airspeed type // @Description: Type of 2nd airspeed sensor @@ -194,7 +201,9 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Values: 0:Bus0(internal),1:Bus1(external),2:Bus2(auxillary) // @User: Advanced AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1), - + + // Note that 21 is used above by the _OPTIONS parameter. Do not use 21. + AP_GROUPEND }; @@ -229,6 +238,9 @@ void AP_Airspeed::init() state[i].calibration.init(param[i].ratio); state[i].last_saved_ratio = param[i].ratio; + // Set the enable automatically to false and set the probability that the airspeed is healhy to start with + state[i].failures.health_probability = 1.0f; + switch ((enum airspeed_type)param[i].type.get()) { case TYPE_NONE: // nothing to do @@ -434,6 +446,13 @@ void AP_Airspeed::update(bool log) } } + // 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 a332049ff0..ccf1da71dd 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -10,6 +10,15 @@ 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; @@ -175,6 +184,7 @@ private: static AP_Airspeed *_singleton; AP_Int8 primary_sensor; + AP_Int32 _options; // bitmask options for airspeed struct { AP_Float offset; @@ -213,6 +223,13 @@ private: Airspeed_Calibration calibration; float last_saved_ratio; uint8_t counter; + + struct { + uint32_t last_check_ms; + float health_probability; + int8_t param_use_backup; + bool has_warned; + } failures; } state[AIRSPEED_MAX_SENSORS]; // current primary sensor @@ -224,6 +241,7 @@ 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(uint8_t i); AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS]; };