5
0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-13 03:13:57 -04:00

Revert "AP_Airspeed: blending of several sensors and failure recognition"

This reverts commit 26c7614118.
This commit is contained in:
Tom Pittenger 2018-03-09 13:12:05 -08:00
parent 585c484be0
commit b1a4540359
2 changed files with 27 additions and 266 deletions
libraries/AP_Airspeed

View File

@ -30,7 +30,6 @@
#include "AP_Airspeed_SDP3X.h" #include "AP_Airspeed_SDP3X.h"
#include "AP_Airspeed_analog.h" #include "AP_Airspeed_analog.h"
#include "AP_Airspeed_Backend.h" #include "AP_Airspeed_Backend.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL &hal; extern const AP_HAL::HAL &hal;
@ -192,25 +191,19 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced // @User: Advanced
AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1), AP_GROUPINFO("2_BUS", 20, AP_Airspeed, param[1].bus, 1),
// @Param: _BLEND
// @DisplayName: Blending of airspeed sensors
// @Description: Sets the configuration for enable the blending of multiple airspeed sensors
// @Values: 0:Blend disabled,1:Full blending
// @User: Advanced
AP_GROUPINFO("_BLEND", 21, AP_Airspeed, blend_enable, 0),
AP_GROUPEND AP_GROUPEND
}; };
AP_Airspeed::AP_Airspeed() AP_Airspeed::AP_Airspeed()
{ {
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) { for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
state[i].EAS2TAS = 1; state[i].EAS2TAS = 1;
state[i].blend_healthy = true;
} }
AP_Param::setup_object_defaults(this, var_info); AP_Param::setup_object_defaults(this, var_info);
} }
/* /*
this scaling factor converts from the old system where we used a this scaling factor converts from the old system where we used a
0 to 4095 raw ADC value for 0-5V to the new system which gets the 0 to 4095 raw ADC value for 0-5V to the new system which gets the
@ -273,9 +266,6 @@ float AP_Airspeed::get_pressure(uint8_t i)
float pressure = 0; float pressure = 0;
if (sensor[i]) { if (sensor[i]) {
state[i].healthy = sensor[i]->get_differential_pressure(pressure); state[i].healthy = sensor[i]->get_differential_pressure(pressure);
if (!state[i].healthy) {
state[i].blend_healthy = false;
}
} }
return pressure; return pressure;
} }
@ -382,7 +372,6 @@ void AP_Airspeed::read(uint8_t i)
// we're reading more than about -8m/s. The user probably has // we're reading more than about -8m/s. The user probably has
// the ports the wrong way around // the ports the wrong way around
state[i].healthy = false; state[i].healthy = false;
state[i].blend_healthy = false;
} }
break; break;
case PITOT_TUBE_ORDER_AUTO: case PITOT_TUBE_ORDER_AUTO:
@ -396,23 +385,6 @@ void AP_Airspeed::read(uint8_t i)
state[i].last_update_ms = AP_HAL::millis(); state[i].last_update_ms = AP_HAL::millis();
} }
void AP_Airspeed::process_no_blend(void)
{
// 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;
}
}
}
blend_state.active = false;
}
// read all airspeed sensors // read all airspeed sensors
void AP_Airspeed::read(void) void AP_Airspeed::read(void)
{ {
@ -427,116 +399,15 @@ void AP_Airspeed::read(void)
} }
#endif #endif
if (blend_enable == BLEND_NONE || // setup primary
!AP::ahrs().have_inertial_nav()) { if (healthy(primary_sensor.get())) {
process_no_blend(); primary = primary_sensor.get();
return; return;
} }
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
// blend the airspeed from several sensors and EKF if (healthy(i)) {
Vector3f velocity, wind; primary = i;
if (AP::ahrs().get_velocity_NED(velocity) == false) { break;
process_no_blend();
return;
}
wind = AP::ahrs().wind_estimate();
velocity -= wind;
float airspeed_ekf = velocity.length();
float ekf_sensor_airspeed_diff[AIRSPEED_MAX_SENSORS];
uint8_t healthy_sens_num = 0;
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
if (!state[i].healthy) {
state[i].blend_healthy = false;
}
if (state[i].blend_healthy) {
healthy_sens_num++;
}
}
// calculating difference
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
ekf_sensor_airspeed_diff[i] = fabsf(state[i].airspeed - airspeed_ekf);
}
if (healthy_sens_num > 0)
{
const float airspeed_avg_limit = airspeed_ekf * AIRSPEED_BLEND_DIFF_LIMIT;
// look for healthy sensors that might be failing
// Idea is to find the sensor which differs much both from other sensors and from ekf value
// If there are two sensors then the first check will be true for both, however
// the second check will be true only for the sensor which really failed
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
if (state[i].blend_healthy &&
ekf_sensor_airspeed_diff[i] > airspeed_avg_limit && state[i].airspeed > AIRSPEED_BLEND_MIN_SPEED) {
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor blending unhealthy", i);
state[i].blend_healthy = false;
}
}
blend_state_t blend_state_temp {};
healthy_sens_num = 0;
uint8_t healthy_sens_temperature_num = 0;
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
if (!state[i].blend_healthy) {
continue;
}
blend_state_temp.airspeed += state[i].airspeed;
blend_state_temp.raw_airspeed += state[i].raw_airspeed;
float temperature;
if (get_temperature(i, temperature)) {
blend_state_temp.temperature += temperature;
healthy_sens_temperature_num++;
}
blend_state_temp.last_pressure += state[i].last_pressure;
blend_state_temp.corrected_pressure += state[i].corrected_pressure;
blend_state_temp.EAS2TAS += state[i].EAS2TAS;
blend_state_temp.ratio += param[i].ratio;
blend_state_temp.offset += param[i].offset;
healthy_sens_num++;
}
if (healthy_sens_num > 0) {
float div2mul_factor = 1.0 / healthy_sens_num;
blend_state_temp.airspeed *= div2mul_factor;
blend_state_temp.raw_airspeed *= div2mul_factor;
blend_state_temp.last_pressure *= div2mul_factor;
blend_state_temp.corrected_pressure *= div2mul_factor;
blend_state_temp.EAS2TAS *= div2mul_factor;
blend_state_temp.ratio *= div2mul_factor;
blend_state_temp.offset *= div2mul_factor;
if (healthy_sens_temperature_num) {
blend_state_temp.temperature /= healthy_sens_temperature_num;
}
blend_state_temp.active = true;
memcpy(&blend_state, &blend_state_temp, sizeof(blend_state));
} else {
process_no_blend();
}
} else
{
process_no_blend();
}
// Check if any unhealthy sensors came back into range
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
if (!state[i].blend_healthy && ekf_sensor_airspeed_diff[i] < AIRSPEED_BLEND_ABS_DIFF_LIMIT &&
state[i].airspeed > AIRSPEED_BLEND_MIN_SPEED && state[i].healthy)
{
gcs().send_text(MAV_SEVERITY_INFO, "Airspeed[%u] sensor blending back healthy", i);
state[i].blend_healthy = true;
} }
} }
} }

View File

@ -7,15 +7,9 @@
#include <AP_Baro/AP_Baro.h> #include <AP_Baro/AP_Baro.h>
class AP_Airspeed_Backend; class AP_Airspeed_Backend;
class AP_AHRS;
#define AIRSPEED_MAX_SENSORS 2 #define AIRSPEED_MAX_SENSORS 2
// set the limit to 40% (TBD)
#define AIRSPEED_BLEND_DIFF_LIMIT 0.4
#define AIRSPEED_BLEND_MIN_SPEED 5.0
#define AIRSPEED_BLEND_ABS_DIFF_LIMIT 5.0
class Airspeed_Calibration { class Airspeed_Calibration {
public: public:
friend class AP_Airspeed; friend class AP_Airspeed;
@ -48,11 +42,6 @@ public:
void init(void); void init(void);
enum blend_type {
BLEND_NONE=0,
BLEND_FULL=1,
};
// read the analog source and update airspeed // read the analog source and update airspeed
void read(void); void read(void);
@ -64,83 +53,39 @@ public:
float get_airspeed(uint8_t i) const { float get_airspeed(uint8_t i) const {
return state[i].airspeed; return state[i].airspeed;
} }
float get_airspeed(void) const { float get_airspeed(void) const { return get_airspeed(primary); }
if (blend_state.active) {
return blend_state.airspeed;
} else {
return get_airspeed(primary);
}
}
// return the unfiltered airspeed in m/s // return the unfiltered airspeed in m/s
float get_raw_airspeed(uint8_t i) const { float get_raw_airspeed(uint8_t i) const {
return state[i].raw_airspeed; return state[i].raw_airspeed;
} }
float get_raw_airspeed(void) const { float get_raw_airspeed(void) const { return get_raw_airspeed(primary); }
if (blend_state.active) {
return blend_state.raw_airspeed;
} else {
return get_raw_airspeed(primary);
}
}
// return the current airspeed ratio (dimensionless) // return the current airspeed ratio (dimensionless)
float get_airspeed_ratio(uint8_t i) const { float get_airspeed_ratio(uint8_t i) const {
return param[i].ratio; return param[i].ratio;
} }
float get_airspeed_ratio(void) const { float get_airspeed_ratio(void) const { return get_airspeed_ratio(primary); }
if (blend_state.active) {
return blend_state.ratio;
} else {
return get_airspeed_ratio(primary);
}
}
// get temperature if available // get temperature if available
bool get_temperature(uint8_t i, float &temperature); bool get_temperature(uint8_t i, float &temperature);
bool get_temperature(float &temperature) { bool get_temperature(float &temperature) { return get_temperature(primary, temperature); }
if (blend_state.active) {
temperature = blend_state.temperature;
return true;
} else {
return get_temperature(primary, temperature);
}
}
// set the airspeed ratio (dimensionless) // set the airspeed ratio (dimensionless)
void set_airspeed_ratio(uint8_t i, float ratio) { void set_airspeed_ratio(uint8_t i, float ratio) {
param[i].ratio.set(ratio); param[i].ratio.set(ratio);
} }
void set_airspeed_ratio(float ratio) { void set_airspeed_ratio(float ratio) { set_airspeed_ratio(primary, ratio); }
if (blend_state.active) {
// TODO: What to do?
set_airspeed_ratio(primary, ratio);
} else {
set_airspeed_ratio(primary, ratio);
}
}
// return true if airspeed is enabled, and airspeed use is set // return true if airspeed is enabled, and airspeed use is set
bool use(uint8_t i) const; bool use(uint8_t i) const;
bool use(void) const { bool use(void) const { return use(primary); }
if (blend_state.active) {
return true;
} else {
return use(primary);
}
}
// return true if airspeed is enabled // return true if airspeed is enabled
bool enabled(uint8_t i) const { bool enabled(uint8_t i) const {
return param[i].type.get() != TYPE_NONE; return param[i].type.get() != TYPE_NONE;
} }
bool enabled(void) const { bool enabled(void) const { return enabled(primary); }
if (blend_state.active) {
return true;
} else {
return enabled(primary);
}
}
// used by HIL to set the airspeed // used by HIL to set the airspeed
void set_HIL(float airspeed) { void set_HIL(float airspeed) {
@ -152,63 +97,31 @@ public:
float get_differential_pressure(uint8_t i) const { float get_differential_pressure(uint8_t i) const {
return state[i].last_pressure; return state[i].last_pressure;
} }
float get_differential_pressure(void) const { float get_differential_pressure(void) const { return get_differential_pressure(primary); }
if (blend_state.active) {
return blend_state.last_pressure;
} else {
return get_differential_pressure(primary);
}
}
// return the current calibration offset // return the current calibration offset
float get_offset(uint8_t i) const { float get_offset(uint8_t i) const {
return param[i].offset; return param[i].offset;
} }
float get_offset(void) const { float get_offset(void) const { return get_offset(primary); }
if (blend_state.active) {
return blend_state.offset;
} else {
return get_offset(primary);
}
}
// return the current corrected pressure // return the current corrected pressure
float get_corrected_pressure(uint8_t i) const { float get_corrected_pressure(uint8_t i) const {
return state[i].corrected_pressure; return state[i].corrected_pressure;
} }
float get_corrected_pressure(void) const { float get_corrected_pressure(void) const { return get_corrected_pressure(primary); }
if (blend_state.active) {
return blend_state.corrected_pressure;
} else {
return get_corrected_pressure(primary);
}
}
// set the apparent to true airspeed ratio // set the apparent to true airspeed ratio
void set_EAS2TAS(uint8_t i, float v) { void set_EAS2TAS(uint8_t i, float v) {
state[i].EAS2TAS = v; state[i].EAS2TAS = v;
} }
void set_EAS2TAS(float v) { void set_EAS2TAS(float v) { set_EAS2TAS(primary, v); }
if (blend_state.active) {
for (uint8_t i = 0; i < AIRSPEED_MAX_SENSORS; i++) {
set_EAS2TAS(i, v);
}
} else {
set_EAS2TAS(primary, v);
}
}
// get the apparent to true airspeed ratio // get the apparent to true airspeed ratio
float get_EAS2TAS(uint8_t i) const { float get_EAS2TAS(uint8_t i) const {
return state[i].EAS2TAS; return state[i].EAS2TAS;
} }
float get_EAS2TAS(void) const { float get_EAS2TAS(void) const { return get_EAS2TAS(primary); }
if (blend_state.active) {
return blend_state.EAS2TAS;
} else {
return get_EAS2TAS(primary);
}
}
// update airspeed ratio calibration // update airspeed ratio calibration
void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
@ -220,13 +133,7 @@ public:
bool healthy(uint8_t i) const { bool healthy(uint8_t i) const {
return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i); return state[i].healthy && (fabsf(param[i].offset) > 0 || state[i].use_zero_offset) && enabled(i);
} }
bool healthy(void) const { bool healthy(void) const { return healthy(primary); }
if (blend_enable == BLEND_FULL) {
return blend_state.active;
} else {
return healthy(primary);
}
}
// return true if all enabled sensors are healthy // return true if all enabled sensors are healthy
bool all_healthy(void) const; bool all_healthy(void) const;
@ -257,11 +164,10 @@ public:
// get current primary sensor // get current primary sensor
uint8_t get_primary(void) const { return primary; } uint8_t get_primary(void) const { return primary; }
private: private:
AP_Int8 primary_sensor; AP_Int8 primary_sensor;
// Blended airspeed
AP_Int8 blend_enable;
struct { struct {
AP_Float offset; AP_Float offset;
@ -275,7 +181,7 @@ private:
AP_Int8 tube_order; AP_Int8 tube_order;
AP_Int8 skip_cal; AP_Int8 skip_cal;
} param[AIRSPEED_MAX_SENSORS]; } param[AIRSPEED_MAX_SENSORS];
struct airspeed_state { struct airspeed_state {
float raw_airspeed; float raw_airspeed;
float airspeed; float airspeed;
@ -284,7 +190,6 @@ private:
float corrected_pressure; float corrected_pressure;
float EAS2TAS; float EAS2TAS;
bool healthy:1; bool healthy:1;
bool blend_healthy:1;
bool hil_set:1; bool hil_set:1;
float hil_pressure; float hil_pressure;
uint32_t last_update_ms; uint32_t last_update_ms;
@ -302,20 +207,7 @@ private:
float last_saved_ratio; float last_saved_ratio;
uint8_t counter; uint8_t counter;
} state[AIRSPEED_MAX_SENSORS]; } state[AIRSPEED_MAX_SENSORS];
struct blend_state_t {
float raw_airspeed;
float airspeed;
float last_pressure;
float corrected_pressure;
float EAS2TAS;
float temperature;
float ratio;
float offset;
bool active;
} blend_state;
// current primary sensor // current primary sensor
uint8_t primary; uint8_t primary;
@ -323,8 +215,6 @@ private:
float get_pressure(uint8_t i); float get_pressure(uint8_t i);
void update_calibration(uint8_t i, float raw_pressure); 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 update_calibration(uint8_t i, const Vector3f &vground, int16_t max_airspeed_allowed_during_cal);
void process_no_blend(void);
AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS]; AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS];
}; };