diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index 966792b61821e..e70b6623d231e 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -304,6 +304,11 @@ class AP_AHRS return _airspeed != nullptr && _airspeed->use() && _airspeed->healthy(); } + // return the parameter AHRS_WIND_MAX in metres per second + uint8_t get_max_wind() const { + return _wind_max; + } + // return a ground vector estimate in meters/second, in North/East order virtual Vector2f groundspeed_vector(void); diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 069064165698d..e17f72903482c 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, 0), + // @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 @@ -427,22 +439,24 @@ void AP_Airspeed::update(bool log) } #endif - if (log) { - AP_Logger *_dataflash = AP_Logger::instance(); - if (_dataflash != nullptr) { - _dataflash->Write_Airspeed(*this); - } - } - // setup primary if (healthy(primary_sensor.get())) { primary = primary_sensor.get(); - return; + } else { + for (uint8_t i=0; iWrite_Airspeed(*this); } } } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index a332049ff01cc..3fa47586141b1 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -125,6 +125,12 @@ class AP_Airspeed } float get_EAS2TAS(void) const { return get_EAS2TAS(primary); } + // get the failure health probability + float get_health_failure_probability(uint8_t i) const { + return state[i].failures.health_probability; + } + float get_health_failure_probability(void) const { return get_health_failure_probability(primary); } + // update airspeed ratio calibration void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); @@ -154,6 +160,11 @@ class AP_Airspeed PITOT_TUBE_ORDER_NEGATIVE = 1, PITOT_TUBE_ORDER_AUTO = 2 }; + enum OptionsMask { + 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 { TYPE_NONE=0, TYPE_I2C_MS4525=1, @@ -175,6 +186,7 @@ class AP_Airspeed static AP_Airspeed *_singleton; AP_Int8 primary_sensor; + AP_Int32 _options; // bitmask options for airspeed struct { AP_Float offset; @@ -213,6 +225,13 @@ class AP_Airspeed 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 @@ -225,5 +244,8 @@ class AP_Airspeed 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_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 new file mode 100644 index 0000000000000..4ca461f988780 --- /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_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); + 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_AHRS_WIND_MAX_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; + } +} diff --git a/libraries/AP_Logger/LogFile.cpp b/libraries/AP_Logger/LogFile.cpp index 3d6a6156881b7..b2059e13f6578 100644 --- a/libraries/AP_Logger/LogFile.cpp +++ b/libraries/AP_Logger/LogFile.cpp @@ -1533,6 +1533,7 @@ void AP_Logger::Write_Airspeed(AP_Airspeed &airspeed) offset : airspeed.get_offset(i), use : airspeed.use(i), healthy : airspeed.healthy(i), + health_prob : airspeed.get_health_failure_probability(i), primary : airspeed.get_primary() }; WriteBlock(&pkt, sizeof(pkt)); diff --git a/libraries/AP_Logger/LogStructure.h b/libraries/AP_Logger/LogStructure.h index 9ef4b779dcf79..5a9dcfafc11ba 100644 --- a/libraries/AP_Logger/LogStructure.h +++ b/libraries/AP_Logger/LogStructure.h @@ -858,6 +858,7 @@ struct PACKED log_AIRSPEED { float offset; bool use; bool healthy; + float health_prob; uint8_t primary; }; @@ -1157,10 +1158,10 @@ struct PACKED log_DSTL { #define CURR_CELL_UNITS "svvvvvvvvvvv" #define CURR_CELL_MULTS "F00000000000" -#define ARSP_LABELS "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset,U,Health,Primary" -#define ARSP_FMT "QffcffBBB" -#define ARSP_UNITS "snPOPP---" -#define ARSP_MULTS "F00B00---" +#define ARSP_LABELS "TimeUS,Airspeed,DiffPress,Temp,RawPress,Offset,U,Health,Hfp,Pri" +#define ARSP_FMT "QffcffBBfB" +#define ARSP_UNITS "snPOPP----" +#define ARSP_MULTS "F00B00----" /* Format characters in the format string for binary log messages