diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 859a5416d0..ffa8f162ae 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -30,7 +30,6 @@ #include "AP_Airspeed_SDP3X.h" #include "AP_Airspeed_analog.h" #include "AP_Airspeed_Backend.h" -#include 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; iget_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 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 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]; };