aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
libraries/AP_Airspeed/AP_Airspeed.cpp
View File

@ -30,7 +30,6 @@
#include "AP_Airspeed_SDP3X.h"
#include "AP_Airspeed_analog.h"
#include "AP_Airspeed_Backend.h"
#include <AP_AHRS/AP_AHRS.h>
extern const AP_HAL::HAL &hal;
@ -192,25 +191,19 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
// @User: Advanced
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_Airspeed::AP_Airspeed()
{
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
state[i].EAS2TAS = 1;
state[i].blend_healthy = true;
}
AP_Param::setup_object_defaults(this, var_info);
}
/*
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
@ -273,9 +266,6 @@ float AP_Airspeed::get_pressure(uint8_t i)
float pressure = 0;
if (sensor[i]) {
state[i].healthy = sensor[i]->get_differential_pressure(pressure);
if (!state[i].healthy) {
state[i].blend_healthy = false;
}
}
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
// the ports the wrong way around
state[i].healthy = false;
state[i].blend_healthy = false;
}
break;
case PITOT_TUBE_ORDER_AUTO:
@ -396,23 +385,6 @@ void AP_Airspeed::read(uint8_t i)
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
void AP_Airspeed::read(void)
{
@ -427,116 +399,15 @@ void AP_Airspeed::read(void)
}
#endif
if (blend_enable == BLEND_NONE ||
!AP::ahrs().have_inertial_nav()) {
process_no_blend();
// setup primary
if (healthy(primary_sensor.get())) {
primary = primary_sensor.get();
return;
}
// blend the airspeed from several sensors and EKF
Vector3f velocity, wind;
if (AP::ahrs().get_velocity_NED(velocity) == false) {
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;
for (uint8_t i=0; i<AIRSPEED_MAX_SENSORS; i++) {
if (healthy(i)) {
primary = i;
break;
}
}
}

146
libraries/AP_Airspeed/AP_Airspeed.h
View File

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