diff --git a/libraries/AP_Baro/AP_Baro_BMP085.cpp b/libraries/AP_Baro/AP_Baro_BMP085.cpp index 3f828c2bd5..50613ed039 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP085.cpp @@ -121,19 +121,16 @@ void AP_Baro_BMP085::accumulate(void) */ void AP_Baro_BMP085::update(void) { - if (_count == 0 && _data_ready()) { + if (!_has_sample && _data_ready()) { accumulate(); } - if (_count == 0) { + if (!_has_sample) { return; } float temperature = 0.1f * _temp; - float pressure = _press_sum / _count; - - _count = 0; - _press_sum = 0; + float pressure = _pressure_filter.getf(); _copy_to_frontend(_instance, pressure, temperature); } @@ -219,13 +216,10 @@ void AP_Baro_BMP085::_calculate() x1 = (p >> 8) * (p >> 8); x1 = (x1 * 3038) >> 16; x2 = (-7357 * p) >> 16; - _press_sum += p + ((x1 + x2 + 3791) >> 4); + p += ((x1 + x2 + 3791) >> 4); - _count++; - if (_count == 254) { - _press_sum *= 0.5f; - _count /= 2; - } + _pressure_filter.apply(p); + _has_sample = true; } bool AP_Baro_BMP085::_data_ready() diff --git a/libraries/AP_Baro/AP_Baro_BMP085.h b/libraries/AP_Baro/AP_Baro_BMP085.h index e080b99feb..7af4c4d017 100644 --- a/libraries/AP_Baro/AP_Baro_BMP085.h +++ b/libraries/AP_Baro/AP_Baro_BMP085.h @@ -4,6 +4,7 @@ #include #include #include +#include #include "AP_Baro_Backend.h" @@ -28,7 +29,7 @@ private: AP_HAL::DigitalSource *_eoc; uint8_t _instance; - uint8_t _count; + bool _has_sample; // Boards with no EOC pin: use times instead uint32_t _last_press_read_command_time; @@ -45,5 +46,5 @@ private: int32_t _raw_pressure; int32_t _raw_temp; int32_t _temp; - float _press_sum; + AverageIntegralFilter _pressure_filter; };