Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Airspeed: Renaming the check method name and readding the overall option check. This will set it up for future options.

This commit is contained in:
ChrisBird 2019-02-03 03:55:38 -08:00 committed by Tom Pittenger
parent 1a04a540f5
commit c9af223579
2 changed files with 9 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_Airspeed/AP_Airspeed.h
View File

@ -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];
};

10
libraries/AP_Airspeed/AP_Airspeed_Health.cpp
View File

@ -6,11 +6,13 @@
void AP_Airspeed::check_sensor_failures()
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
check_sensor_failures(i);
if(AP_Airspeed::OptionsMask::ON_FAILURE_AHRS_WIND_MAX_DO_DISABLE & _options) {
check_sensor_ahrs_wind_max_failures(i);
}
}
}
void AP_Airspeed::check_sensor_failures(uint8_t i)
void AP_Airspeed::check_sensor_ahrs_wind_max_failures(uint8_t i)
{
const uint32_t now_ms = AP_HAL::millis();
if ((now_ms - state[i].failures.last_check_ms) <= 200) {
@ -55,7 +57,7 @@ void AP_Airspeed::check_sensor_failures(uint8_t i)
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)) {
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) {