From a39cfda192b330986a1b32e8483e30949cbe98a7 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 19 Sep 2017 13:28:56 -0600 Subject: [PATCH] AP_Airspeed: eliminate airspeed positive bias --- libraries/AP_Airspeed/AP_Airspeed.cpp | 23 +++++++++++++++-------- libraries/AP_Airspeed/AP_Airspeed.h | 1 + 2 files changed, 16 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 07edacaf56..13c47b361d 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -256,15 +256,23 @@ void AP_Airspeed::read(void) // remember raw pressure for logging _corrected_pressure = airspeed_pressure; + // filter before clamping positive + _filtered_pressure = 0.7f * _filtered_pressure + 0.3f * airspeed_pressure; + /* - we support different pitot tube setups so used can choose if + we support different pitot tube setups so user can choose if they want to be able to detect pressure on the static port */ switch ((enum pitot_tube_order)_tube_order.get()) { case PITOT_TUBE_ORDER_NEGATIVE: - airspeed_pressure = -airspeed_pressure; - // no break + _last_pressure = -airspeed_pressure; + _raw_airspeed = sqrtf(MAX(-airspeed_pressure, 0) * _ratio); + _airspeed = sqrtf(MAX(-_filtered_pressure, 0) * _ratio); + break; case PITOT_TUBE_ORDER_POSITIVE: + _last_pressure = airspeed_pressure; + _raw_airspeed = sqrtf(MAX(airspeed_pressure, 0) * _ratio); + _airspeed = sqrtf(MAX(_filtered_pressure, 0) * _ratio); if (airspeed_pressure < -32) { // we're reading more than about -8m/s. The user probably has // the ports the wrong way around @@ -273,13 +281,12 @@ void AP_Airspeed::read(void) break; case PITOT_TUBE_ORDER_AUTO: default: - airspeed_pressure = fabsf(airspeed_pressure); + _last_pressure = fabsf(airspeed_pressure); + _raw_airspeed = sqrtf(fabsf(airspeed_pressure) * _ratio); + _airspeed = sqrtf(fabsf(_filtered_pressure) * _ratio); break; } - airspeed_pressure = MAX(airspeed_pressure, 0); - _last_pressure = airspeed_pressure; - _raw_airspeed = sqrtf(airspeed_pressure * _ratio); - _airspeed = 0.7f * _airspeed + 0.3f * _raw_airspeed; + _last_update_ms = AP_HAL::millis(); } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index abc0e6b9d8..cb22e66c1e 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -161,6 +161,7 @@ private: float _raw_airspeed; float _airspeed; float _last_pressure; + float _filtered_pressure; float _corrected_pressure; float _EAS2TAS; bool _healthy:1;