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 }; PITOT_TUBE_ORDER_AUTO = 2 };
enum OptionsMask { enum OptionsMask {
ON_FAILURE_DO_DISABLE = (1<<0), // If set then use airspeed failure check ON_FAILURE_AHRS_WIND_MAX_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_RECOVERY_DO_REENABLE = (1<<1), // If set then automatically enable the airspeed sensor use when healthy again.
}; };
enum airspeed_type { 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 update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
void check_sensor_failures(); 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]; 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() void AP_Airspeed::check_sensor_failures()
{ {
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { 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(); const uint32_t now_ms = AP_HAL::millis();
if ((now_ms - state[i].failures.last_check_ms) <= 200) { 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; static const float RE_ENABLE_PROB_THRESH_OK = 0.95f;
// if "disable" option is allowed and sensor is enabled // 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 // and is probably not healthy
if (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT) { if (state[i].failures.health_probability < DISABLE_PROB_THRESH_CRIT) {
gcs().send_text(MAV_SEVERITY_ERROR, "Airspeed sensor %d failure. Disabling", i+1); 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 // if Re-Enable options is allowed, and sensor is disabled but was previously enabled, and is probably healthy
} else if (param[i].use == 0 && } 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.param_use_backup > 0 &&
state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK) { state[i].failures.health_probability > RE_ENABLE_PROB_THRESH_OK) {