From 47be6d8ad17e23dfce813e8ae0603ad8d9119c06 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 15 Aug 2012 12:34:08 +1000 Subject: [PATCH] AP_Airspeed: remove airspeed filter and run at 10Hz this saves on the filter memory, and gives just as good a result --- ArduPlane/ArduPlane.pde | 16 ++++++++-------- libraries/AP_Airspeed/AP_Airspeed.cpp | 2 +- libraries/AP_Airspeed/AP_Airspeed.h | 1 - 3 files changed, 9 insertions(+), 10 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 69b1086aa1..292b69340e 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -761,14 +761,6 @@ static void fast_loop() // ------------------------------------ check_short_failsafe(); - // Read Airspeed - // ------------- - if (airspeed.enabled()) { -#if HIL_MODE != HIL_MODE_ATTITUDE - read_airspeed(); -#endif - } - #if HIL_MODE == HIL_MODE_SENSORS // update hil before AHRS update gcs_update(); @@ -878,6 +870,14 @@ Serial.println(tempaccel.z, DEC); case 2: medium_loopCounter++; + // Read Airspeed + // ------------- +#if HIL_MODE != HIL_MODE_ATTITUDE + if (airspeed.enabled()) { + read_airspeed(); + } +#endif + // Read altitude from sensors // ------------------ update_alt(); diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 8def149969..728025067b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -69,7 +69,7 @@ void AP_Airspeed::read(void) if (!_enable) { return; } - _airspeed_raw = _filter.apply(_source->read()); + _airspeed_raw = _source->read(); airspeed_pressure = max((_airspeed_raw - _offset), 0); _airspeed = sqrt(airspeed_pressure * _ratio); } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 350dc6954d..5ab9d86111 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -50,7 +50,6 @@ private: AP_Int8 _enable; float _airspeed; float _airspeed_raw; - AverageFilterFloat_Size5 _filter; };