diff --git a/libraries/AP_Baro/AP_Baro_BMP280.cpp b/libraries/AP_Baro/AP_Baro_BMP280.cpp index ff533f348a..d6823ee2fb 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP280.cpp @@ -72,8 +72,6 @@ bool AP_Baro_BMP280::_init() } WITH_SEMAPHORE(_dev->get_semaphore()); - _has_sample = false; - _dev->set_speed(AP_HAL::Device::SPEED_HIGH); uint8_t whoami; @@ -144,12 +142,13 @@ void AP_Baro_BMP280::update(void) { WITH_SEMAPHORE(_sem); - if (!_has_sample) { + if (_pressure_count == 0) { return; } - _copy_to_frontend(_instance, _pressure, _temperature); - _has_sample = false; + _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature); + _pressure_count = 0; + _pressure_sum = 0; } // calculate temperature @@ -201,6 +200,6 @@ void AP_Baro_BMP280::_update_pressure(int32_t press_raw) WITH_SEMAPHORE(_sem); - _pressure = press; - _has_sample = true; + _pressure_sum += press; + _pressure_count++; } diff --git a/libraries/AP_Baro/AP_Baro_BMP280.h b/libraries/AP_Baro/AP_Baro_BMP280.h index f8869c1a10..98e81a32eb 100644 --- a/libraries/AP_Baro/AP_Baro_BMP280.h +++ b/libraries/AP_Baro/AP_Baro_BMP280.h @@ -32,10 +32,10 @@ class AP_Baro_BMP280 : public AP_Baro_Backend AP_HAL::OwnPtr _dev; - bool _has_sample; uint8_t _instance; int32_t _t_fine; - float _pressure; + float _pressure_sum; + uint32_t _pressure_count; float _temperature; // Internal calibration registers diff --git a/libraries/AP_Baro/AP_Baro_BMP388.cpp b/libraries/AP_Baro/AP_Baro_BMP388.cpp index 83942e07ea..7eb4e31005 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.cpp +++ b/libraries/AP_Baro/AP_Baro_BMP388.cpp @@ -76,8 +76,6 @@ bool AP_Baro_BMP388::init() } WITH_SEMAPHORE(dev->get_semaphore()); - has_sample = false; - dev->set_speed(AP_HAL::Device::SPEED_HIGH); // setup to allow reads on SPI @@ -145,12 +143,16 @@ void AP_Baro_BMP388::update(void) { WITH_SEMAPHORE(_sem); - if (!has_sample) { + if (pressure_count == 0) { return; } - _copy_to_frontend(instance, pressure, temperature); - has_sample = false; + _copy_to_frontend(instance, + pressure_sum/pressure_count, + temperature); + + pressure_sum = 0; + pressure_count = 0; } /* @@ -211,9 +213,9 @@ void AP_Baro_BMP388::update_pressure(uint32_t data) float press = partial_out1 + partial_out2 + partial4; WITH_SEMAPHORE(_sem); - - pressure = press; - has_sample = true; + + pressure_sum += press; + pressure_count++; } /* diff --git a/libraries/AP_Baro/AP_Baro_BMP388.h b/libraries/AP_Baro/AP_Baro_BMP388.h index ccb24c14d1..b854da246f 100644 --- a/libraries/AP_Baro/AP_Baro_BMP388.h +++ b/libraries/AP_Baro/AP_Baro_BMP388.h @@ -32,9 +32,9 @@ class AP_Baro_BMP388 : public AP_Baro_Backend AP_HAL::OwnPtr dev; - bool has_sample; uint8_t instance; - float pressure; + float pressure_sum; + uint32_t pressure_count; float temperature; // Internal calibration registers diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp index f43b51eea6..aed59ef9dd 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.cpp +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.cpp @@ -141,8 +141,6 @@ bool AP_Baro_LPS2XH::_init() } _dev->get_semaphore()->take_blocking(); - _has_sample = false; - _dev->set_speed(AP_HAL::Device::SPEED_HIGH); // top bit is for read on SPI @@ -226,21 +224,19 @@ void AP_Baro_LPS2XH::_timer(void) if (status & 0x01) { _update_pressure(); } - - _has_sample = true; } // transfer data to the frontend void AP_Baro_LPS2XH::update(void) { - if (!_has_sample) { + if (_pressure_count == 0) { return; } WITH_SEMAPHORE(_sem); - _copy_to_frontend(_instance, _pressure, _temperature); - - _has_sample = false; + _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature); + _pressure_sum = 0; + _pressure_count = 0; } // calculate temperature @@ -275,5 +271,6 @@ void AP_Baro_LPS2XH::_update_pressure(void) int32_t Pressure_mb = Pressure_Reg_s32 * (100.0f / 4096); // scale for pa WITH_SEMAPHORE(_sem); - _pressure = Pressure_mb; + _pressure_sum += Pressure_mb; + _pressure_count++; } diff --git a/libraries/AP_Baro/AP_Baro_LPS2XH.h b/libraries/AP_Baro/AP_Baro_LPS2XH.h index 969a55e537..f086d77fff 100644 --- a/libraries/AP_Baro/AP_Baro_LPS2XH.h +++ b/libraries/AP_Baro/AP_Baro_LPS2XH.h @@ -42,9 +42,9 @@ class AP_Baro_LPS2XH : public AP_Baro_Backend AP_HAL::OwnPtr _dev; - bool _has_sample; uint8_t _instance; - float _pressure; + float _pressure_sum; + uint32_t _pressure_count; float _temperature; uint32_t CallTime = 0; diff --git a/libraries/AP_Baro/AP_Baro_SPL06.cpp b/libraries/AP_Baro/AP_Baro_SPL06.cpp index 40113668e2..fe29dfc2a3 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.cpp +++ b/libraries/AP_Baro/AP_Baro_SPL06.cpp @@ -98,8 +98,6 @@ bool AP_Baro_SPL06::_init() } WITH_SEMAPHORE(_dev->get_semaphore()); - _has_sample = false; - _dev->set_speed(AP_HAL::Device::SPEED_HIGH); uint8_t whoami; @@ -209,12 +207,14 @@ void AP_Baro_SPL06::update(void) { WITH_SEMAPHORE(_sem); - if (!_has_sample) { + if (_pressure_count == 0) { return; } - _copy_to_frontend(_instance, _pressure, _temperature); - _has_sample = false; + _copy_to_frontend(_instance, _pressure_sum/_pressure_count, _temperature); + + _pressure_sum = 0; + _pressure_count = 0; } // calculate temperature @@ -243,6 +243,6 @@ void AP_Baro_SPL06::_update_pressure(int32_t press_raw) WITH_SEMAPHORE(_sem); - _pressure = press_comp; - _has_sample = true; + _pressure_sum += press_comp; + _pressure_count++; } diff --git a/libraries/AP_Baro/AP_Baro_SPL06.h b/libraries/AP_Baro/AP_Baro_SPL06.h index 0ab62c2d13..c57cb7e829 100644 --- a/libraries/AP_Baro/AP_Baro_SPL06.h +++ b/libraries/AP_Baro/AP_Baro_SPL06.h @@ -35,10 +35,10 @@ class AP_Baro_SPL06 : public AP_Baro_Backend AP_HAL::OwnPtr _dev; int8_t _timer_counter; - bool _has_sample; uint8_t _instance; float _temp_raw; - float _pressure; + float _pressure_sum; + uint32_t _pressure_count; float _temperature; // Internal calibration registers