mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-11 17:13:56 -03:00
AP_Airspeed: convert the defines into static consts
- also perform logging at end of update - convert Options Mask into an enum
This commit is contained in:
parent
d77906cb06
commit
1a04a540f5
@ -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; i<AIRSPEED_MAX_SENSORS; i++) {
|
||||
if (healthy(i)) {
|
||||
primary = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
check_sensor_failures();
|
||||
|
||||
if (log) {
|
||||
AP_Logger *_dataflash = AP_Logger::instance();
|
||||
if (_dataflash != nullptr) {
|
||||
_dataflash->Write_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<AIRSPEED_MAX_SENSORS; i++) {
|
||||
check_sensor_failures(i);
|
||||
}
|
||||
}
|
||||
|
||||
// setup primary
|
||||
if (healthy(primary_sensor.get())) {
|
||||
primary = primary_sensor.get();
|
||||
return;
|
||||
}
|
||||
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
|
||||
if (healthy(i)) {
|
||||
primary = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Airspeed::setHIL(float airspeed, float diff_pressure, float temperature)
|
||||
@ -504,75 +499,5 @@ bool AP_Airspeed::all_healthy(void) const
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void AP_Airspeed::check_sensor_failures(uint8_t i)
|
||||
{
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if ((now_ms - state[i].failures.last_check_ms) <= AP_AIRSPEED_OPTIONS_FAILURE_SAMPLE_PERIOD_MS) {
|
||||
// slow the checking rate
|
||||
return;
|
||||
}
|
||||
|
||||
const float aspeed = get_airspeed();
|
||||
const float wind_max = AP::ahrs().get_max_wind();
|
||||
|
||||
if (aspeed <= 0 || wind_max <= 0) {
|
||||
// invalid estimates
|
||||
return;
|
||||
}
|
||||
|
||||
state[i].failures.last_check_ms = now_ms;
|
||||
|
||||
// update state[i].failures.health_probability via LowPassFilter
|
||||
if (AP::gps().status() >= 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;
|
||||
|
@ -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];
|
||||
|
86
libraries/AP_Airspeed/AP_Airspeed_Health.cpp
Normal file
86
libraries/AP_Airspeed/AP_Airspeed_Health.cpp
Normal file
@ -0,0 +1,86 @@
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include "AP_Airspeed.h"
|
||||
|
||||
void AP_Airspeed::check_sensor_failures()
|
||||
{
|
||||
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
|
||||
check_sensor_failures(i);
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Airspeed::check_sensor_failures(uint8_t i)
|
||||
{
|
||||
const uint32_t now_ms = AP_HAL::millis();
|
||||
if ((now_ms - state[i].failures.last_check_ms) <= 200) {
|
||||
// slow the checking rate
|
||||
return;
|
||||
}
|
||||
|
||||
const float aspeed = get_airspeed();
|
||||
const float wind_max = AP::ahrs().get_max_wind();
|
||||
|
||||
if (aspeed <= 0 || wind_max <= 0) {
|
||||
// invalid estimates
|
||||
return;
|
||||
}
|
||||
|
||||
state[i].failures.last_check_ms = now_ms;
|
||||
|
||||
// update state[i].failures.health_probability via LowPassFilter
|
||||
float speed_accuracy;
|
||||
if (AP::gps().speed_accuracy(speed_accuracy)) {
|
||||
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
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue
Block a user