diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index ffa8f162ae..859a5416d0 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -30,6 +30,7 @@ #include "AP_Airspeed_SDP3X.h" #include "AP_Airspeed_analog.h" #include "AP_Airspeed_Backend.h" +#include extern const AP_HAL::HAL &hal; @@ -191,19 +192,25 @@ 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; iget_differential_pressure(pressure); + if (!state[i].healthy) { + state[i].blend_healthy = false; + } } return pressure; } @@ -372,6 +382,7 @@ 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: @@ -385,6 +396,23 @@ 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 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; } } } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 65df10d31e..b09711ba1a 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -7,9 +7,15 @@ #include 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; @@ -42,6 +48,11 @@ public: void init(void); + enum blend_type { + BLEND_NONE=0, + BLEND_FULL=1, + }; + // read the analog source and update airspeed void read(void); @@ -53,39 +64,83 @@ public: float get_airspeed(uint8_t i) const { return state[i].airspeed; } - float get_airspeed(void) const { return get_airspeed(primary); } + float get_airspeed(void) const { + if (blend_state.active) { + return blend_state.airspeed; + } else { + 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 { return get_raw_airspeed(primary); } + float get_raw_airspeed(void) const { + if (blend_state.active) { + return blend_state.raw_airspeed; + } else { + 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 { return get_airspeed_ratio(primary); } + float get_airspeed_ratio(void) const { + if (blend_state.active) { + return blend_state.ratio; + } else { + return get_airspeed_ratio(primary); + } + } // get temperature if available bool get_temperature(uint8_t i, float &temperature); - bool get_temperature(float &temperature) { return get_temperature(primary, temperature); } + bool get_temperature(float &temperature) { + if (blend_state.active) { + temperature = blend_state.temperature; + return true; + } else { + 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) { set_airspeed_ratio(primary, 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); + } + } // return true if airspeed is enabled, and airspeed use is set bool use(uint8_t i) const; - bool use(void) const { return use(primary); } + bool use(void) const { + if (blend_state.active) { + return true; + } else { + 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 { return enabled(primary); } + bool enabled(void) const { + if (blend_state.active) { + return true; + } else { + return enabled(primary); + } + } // used by HIL to set the airspeed void set_HIL(float airspeed) { @@ -97,31 +152,63 @@ public: float get_differential_pressure(uint8_t i) const { return state[i].last_pressure; } - float get_differential_pressure(void) const { return get_differential_pressure(primary); } + float get_differential_pressure(void) const { + if (blend_state.active) { + return blend_state.last_pressure; + } else { + 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 { return get_offset(primary); } + float get_offset(void) const { + if (blend_state.active) { + return blend_state.offset; + } else { + 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 { return get_corrected_pressure(primary); } + float get_corrected_pressure(void) const { + if (blend_state.active) { + return blend_state.corrected_pressure; + } else { + 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) { set_EAS2TAS(primary, 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); + } + } // get the apparent to true airspeed ratio float get_EAS2TAS(uint8_t i) const { return state[i].EAS2TAS; } - float get_EAS2TAS(void) const { return get_EAS2TAS(primary); } + float get_EAS2TAS(void) const { + if (blend_state.active) { + return blend_state.EAS2TAS; + } else { + return get_EAS2TAS(primary); + } + } // update airspeed ratio calibration void update_calibration(const Vector3f &vground, int16_t max_airspeed_allowed_during_cal); @@ -133,7 +220,13 @@ 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 { return healthy(primary); } + bool healthy(void) const { + if (blend_enable == BLEND_FULL) { + return blend_state.active; + } else { + return healthy(primary); + } + } // return true if all enabled sensors are healthy bool all_healthy(void) const; @@ -164,10 +257,11 @@ public: // get current primary sensor uint8_t get_primary(void) const { return primary; } - -private: +private: AP_Int8 primary_sensor; + // Blended airspeed + AP_Int8 blend_enable; struct { AP_Float offset; @@ -181,7 +275,7 @@ private: AP_Int8 tube_order; AP_Int8 skip_cal; } param[AIRSPEED_MAX_SENSORS]; - + struct airspeed_state { float raw_airspeed; float airspeed; @@ -190,6 +284,7 @@ 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; @@ -207,7 +302,20 @@ 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; @@ -215,6 +323,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 process_no_blend(void); + AP_Airspeed_Backend *sensor[AIRSPEED_MAX_SENSORS]; };