diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 5c2bfccdf3..02320cfefe 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -8,8 +8,7 @@ * of the License, or (at your option) any later version. */ -#include - +#include #include #include #include @@ -55,10 +54,10 @@ void AP_Airspeed::calibrate() if (!_enable) { return; } - _source->read(); + _source->read_average(); for (c = 0; c < 10; c++) { hal.scheduler->delay(100); - sum += _source->read(); + sum += _source->read_average(); } _airspeed_raw = sum/c; _offset.set_and_save(_airspeed_raw); @@ -72,9 +71,8 @@ void AP_Airspeed::read(void) if (!_enable) { return; } - _airspeed_raw = _source->read(); - airspeed_pressure = ((_airspeed_raw - _offset) < 0) ? - (_airspeed_raw - _offset) : 0; + _airspeed_raw = _source->read_average(); + airspeed_pressure = max(_airspeed_raw - _offset, 0); _airspeed = 0.7 * _airspeed + 0.3 * sqrt(airspeed_pressure * _ratio); } diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 5e5c63fe05..0bbd204f5b 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -21,7 +21,6 @@ public: // calibrate the airspeed. This must be called on startup if the // altitude/climb_rate/acceleration interfaces are ever used - // the callback is a delay() like routine void calibrate(); // return the current airspeed in m/s