AP_Baro: added range filter to backend, use it in most sensors (thanks khancyr for style correction)

libraries/AP_Baro/AP_Baro.cpp

@@ -20,6 +20,7 @@
 #include "AP_Baro.h"
 
 #include <utility>
+#include <stdio.h>
 
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Common/AP_Common.h>
@@ -51,6 +52,9 @@
 
 #define INTERNAL_TEMPERATURE_CLAMP 35.0f
 
+#ifndef HAL_BARO_FILTER_DEFAULT
+ #define HAL_BARO_FILTER_DEFAULT 0 // turned off by default
+#endif
 
 extern const AP_HAL::HAL& hal;
 
@@ -137,6 +141,14 @@ const AP_Param::GroupInfo AP_Baro::var_info[] = {
     // Slot 12 used to be TEMP3
 #endif
 
+    // @Param: FLTR_RNG
+    // @DisplayName: Range in which sample is accepted
+    // @Description: This sets the range around the average value that new samples must be within to be accepted. This can help reduce the impact of noise on sensors that are on long I2C cables. The value is a percentage from the average value. A value of zero disables this filter.
+    // @Units: %
+    // @Range: 0 100
+    // @Increment: 1
+    AP_GROUPINFO("FLTR_RNG", 13, AP_Baro, _filter_range, HAL_BARO_FILTER_DEFAULT),
+
     AP_GROUPEND
 };
 

libraries/AP_Baro/AP_Baro.h

@@ -172,6 +172,8 @@ public:
     // set a pressure correction from AP_TempCalibration
     void set_pressure_correction(uint8_t instance, float p_correction);
 
+    uint8_t get_filter_range() const { return _filter_range; }
+
 private:
     // singleton
     static AP_Baro *_instance;
@@ -218,6 +220,7 @@ private:
     uint32_t                            _last_notify_ms;
 
     bool _add_backend(AP_Baro_Backend *backend);
+    AP_Int8                            _filter_range;  // valid value range from mean value
 };
 
 namespace AP {

libraries/AP_Baro/AP_Baro_BMP085.cpp

@@ -15,6 +15,7 @@
 #include "AP_Baro_BMP085.h"
 
 #include <utility>
+#include <stdio.h>
 
 #include <AP_Common/AP_Common.h>
 #include <AP_HAL/AP_HAL.h>
@@ -307,6 +308,10 @@ void AP_Baro_BMP085::_calculate()
     x2 = (-7357 * p) >> 16;
     p += ((x1 + x2 + 3791) >> 4);
 
+    if (!pressure_ok(p)) {
+        return;
+    }
+
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         _pressure_filter.apply(p);
         _has_sample = true;

libraries/AP_Baro/AP_Baro_BMP280.cpp

@@ -158,8 +158,11 @@ void AP_Baro_BMP280::_update_temperature(int32_t temp_raw)
     var2 = (((((temp_raw >> 4) - ((int32_t)_t1)) * ((temp_raw >> 4) - ((int32_t)_t1))) >> 12) * ((int32_t)_t3)) >> 14;
     _t_fine = var1 + var2;
     t = (_t_fine * 5 + 128) >> 8;
+
+    const float temp = ((float)t) / 100.0f;
+
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _temperature = ((float)t) / 100;
+        _temperature = temp;
         _sem->give();
     }
 }
@@ -187,8 +190,13 @@ void AP_Baro_BMP280::_update_pressure(int32_t press_raw)
     var2 = (((int64_t)_p8) * p) >> 19;
     p = ((p + var1 + var2) >> 8) + (((int64_t)_p7) << 4);
 
+
+    const float press = (float)p / 25600.0f;
+    if (!pressure_ok(press)) {
+        return;
+    }
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _pressure = (float)p / 25600;
+        _pressure = press;
         _has_sample = true;
         _sem->give();
     }

libraries/AP_Baro/AP_Baro_Backend.cpp

@@ -1,4 +1,5 @@
 #include "AP_Baro_Backend.h"
+#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -56,3 +57,39 @@ void AP_Baro_Backend::_copy_to_frontend(uint8_t instance, float pressure, float
     _frontend.sensors[instance].temperature = temperature;
     _frontend.sensors[instance].last_update_ms = now;
 }
+
+static constexpr float FILTER_KOEF = 0.1f;
+
+/* Check that the baro value is valid by using a mean filter. If the
+ * value is further than filtrer_range from mean value, it is
+ * rejected. 
+*/
+bool AP_Baro_Backend::pressure_ok(float press)
+{
+    
+    if (isinf(press) || isnan(press)) {
+        return false;
+    }
+
+    const float range = (float)_frontend.get_filter_range();
+    if (range <= 0) {
+        return true;
+    }
+
+    bool ret = true;
+    if (is_zero(_mean_pressure)) {
+        _mean_pressure = press;
+    } else {
+        const float d = fabsf(_mean_pressure - press) / (_mean_pressure + press);  // diff divide by mean value in percent ( with the * 200.0f on later line)
+        float koeff = FILTER_KOEF;
+
+        if (d * 200.0f > range) {  // check the difference from mean value outside allowed range
+            // printf("\nBaro pressure error: mean %f got %f\n", (double)_mean_pressure, (double)press );
+            ret = false;
+            koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
+            _error_count++;
+        }
+        _mean_pressure = _mean_pressure * (1 - koeff) + press * koeff; // complimentary filter 1/k
+    }
+    return ret;
+}

libraries/AP_Baro/AP_Baro_Backend.h

@@ -22,6 +22,10 @@ public:
 
     void backend_update(uint8_t instance);
 
+    //  Check that the baro valid by using a mean filter.
+    // If the value further that filtrer_range from mean value, it is rejected.
+    bool pressure_ok(float press);
+    uint32_t get_error_count() const { return _error_count; }
 protected:
     // reference to frontend object
     AP_Baro &_frontend;
@@ -33,4 +37,8 @@ protected:
 
     virtual void update_healthy_flag(uint8_t instance);
 
+    // mean pressure for range filter
+    float _mean_pressure; 
+    // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor
+    uint32_t _error_count;
 };

libraries/AP_Baro/AP_Baro_DPS280.cpp

@@ -205,6 +205,11 @@ void AP_Baro_DPS280::timer(void)
     float pressure, temperature;
 
     calculate_PT(temp, press, pressure, temperature);
+
+    if (!pressure_ok(pressure)) {
+        return;
+    }
+
     if (_sem->take_nonblocking()) {
         pressure_sum += pressure;
         temperature_sum += temperature;

libraries/AP_Baro/AP_Baro_FBM320.cpp

@@ -187,7 +187,7 @@ void AP_Baro_FBM320::timer(void)
     } else {
         int32_t pressure, temperature;
         calculate_PT(value_T, value, pressure, temperature);
-        if (_sem->take_nonblocking()) {
+        if (pressure_ok(pressure) && _sem->take_nonblocking()) {
             pressure_sum += pressure;
             // sum and convert to degrees
             temperature_sum += temperature*0.01;

libraries/AP_Baro/AP_Baro_ICM20789.cpp

@@ -301,8 +301,7 @@ void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw)
     // pressure involves a few more calculations
     float P = get_pressure(Praw, Traw);
 
-    if (isinf(P) || isnan(P)) {
-        // really bad data
+    if (!pressure_ok(P)) {
         return;
     }
 
@@ -313,7 +312,7 @@ void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw)
         dd.P = P;
         dd.T = T;
 #endif
-
+        
         accum.psum += P;
         accum.tsum += T;
         accum.count++;

libraries/AP_Baro/AP_Baro_KellerLD.cpp

@@ -188,12 +188,14 @@ bool AP_Baro_KellerLD::_read()
         return false;
     }
 
+    if (!pressure_ok(pressure_raw)) {
+        return false;
+    }
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         _update_and_wrap_accumulator(pressure_raw, temperature_raw, 128);
         _sem->give();
         return true;
     }
-
     return false;
 }
 

libraries/AP_Baro/AP_Baro_MS5611.cpp

@@ -309,8 +309,10 @@ void AP_Baro_MS56XX::_timer(void)
             _update_and_wrap_accumulator(&_accum.s_D2, adc_val,
                                          &_accum.d2_count, 32);
         } else {
-            _update_and_wrap_accumulator(&_accum.s_D1, adc_val,
-                                         &_accum.d1_count, 128);
+            if (pressure_ok(adc_val)) {
+                _update_and_wrap_accumulator(&_accum.s_D1, adc_val,
+                                             &_accum.d1_count, 128);
+            }
         }
         _sem->give();
         _state = next_state;
@@ -506,5 +508,6 @@ void AP_Baro_MS56XX::_calculate_5837()
     int32_t pressure = ((int64_t)raw_pressure * SENS / (int64_t)2097152 - OFF) / (int64_t)8192;
     pressure = pressure * 10; // MS5837 only reports to 0.1 mbar
     float temperature = TEMP * 0.01f;
+
     _copy_to_frontend(_instance, (float)pressure, temperature);
 }