AP_Baro: fixed averaging of samples for several drivers

ArduCustom · May 7, 2021 · 901978a · 901978a
1 parent 04aed49
commit 901978a
Show file tree

Hide file tree

Showing 8 changed files with 37 additions and 39 deletions.
diff --git 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
@@ -32,10 +32,10 @@ class AP_Baro_BMP280 : public AP_Baro_Backend
 
     AP_HAL::OwnPtr<AP_HAL::Device> _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
@@ -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
@@ -32,9 +32,9 @@ class AP_Baro_BMP388 : public AP_Baro_Backend
 
     AP_HAL::OwnPtr<AP_HAL::Device> 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
@@ -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
@@ -42,9 +42,9 @@ class AP_Baro_LPS2XH : public AP_Baro_Backend
 
     AP_HAL::OwnPtr<AP_HAL::Device> _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
@@ -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
@@ -35,10 +35,10 @@ class AP_Baro_SPL06 : public AP_Baro_Backend
     AP_HAL::OwnPtr<AP_HAL::Device> _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