AP_Compass: added range filter to backend, added its use in some sensors (thanks khancyr for style correction)

libraries/AP_Compass/AP_Compass.cpp

@@ -39,6 +39,10 @@ extern AP_HAL::HAL& hal;
 #define AP_COMPASS_OFFSETS_MAX_DEFAULT 850
 #endif
 
+#ifndef HAL_COMPASS_FILTER_DEFAULT
+ #define HAL_COMPASS_FILTER_DEFAULT 0 // turned off by default
+#endif
+
 const AP_Param::GroupInfo Compass::var_info[] = {
     // index 0 was used for the old orientation matrix
 
@@ -433,6 +437,14 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Bitmask: 0:HMC5883,1:LSM303D,2:AK8963,3:BMM150,4:LSM9DS1,5:LIS3MDL,6:AK09916,7:IST8310,8:ICM20948,9:MMC3416,10:QFLIGHT,11:UAVCAN,12:QMC5883
     // @User: Advanced
     AP_GROUPINFO("TYPEMASK", 33, Compass, _driver_type_mask, 0),
+
+    // @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", 34, Compass, _filter_range, HAL_COMPASS_FILTER_DEFAULT),
     
     AP_GROUPEND
 };

libraries/AP_Compass/AP_Compass.h

@@ -322,6 +322,8 @@ public:
         return (uint16_t)_offset_max.get();
     }
 
+    uint8_t get_filter_range() const { return uint8_t(_filter_range.get()); }
+
 private:
     /// Register a new compas driver, allocating an instance number
     ///
@@ -460,4 +462,6 @@ private:
 
     // mask of driver types to not load. Bit positions match DEVTYPE_ in backend
     AP_Int32 _driver_type_mask;
+    
+    AP_Int8 _filter_range;
 };

libraries/AP_Compass/AP_Compass_Backend.cpp

@@ -2,6 +2,7 @@
 
 #include "AP_Compass.h"
 #include "AP_Compass_Backend.h"
+#include <stdio.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -147,3 +148,45 @@ void AP_Compass_Backend::set_rotation(uint8_t instance, enum Rotation rotation)
 {
     _compass._state[instance].rotation = rotation;
 }
+
+static constexpr float FILTER_KOEF = 0.1f;
+
+/* Check that the compass value is valid by using a mean filter. If
+ * the value is further than filtrer_range from mean value, it is
+ * rejected. 
+*/
+bool AP_Compass_Backend::field_ok(const Vector3f &field)
+{
+
+    
+    if (field.is_inf() || field.is_nan()) {
+        return false;
+    }
+
+    const float range = (float)_compass.get_filter_range();
+    if (range == 0) {
+        return true;
+    }
+
+    const float length = field.length();
+
+    if (is_zero(_mean_field_length)) {
+        _mean_field_length = length;
+        return true;
+    }
+
+    bool ret = true;
+    const float d = fabsf(_mean_field_length - length) / (_mean_field_length + length);  // 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("\nCompass field length error: mean %f got %f\n", (double)_mean_field_length, (double)length );
+        ret = false;
+        koeff /= (d * 10.0f);  // 2.5 and more, so one bad sample never change mean more than 4%
+        _error_count++;
+    }
+    _mean_field_length = _mean_field_length * (1 - koeff) + length * koeff;  // complimentary filter 1/k
+
+    return ret;
+}
+

libraries/AP_Compass/AP_Compass_Backend.h

@@ -108,6 +108,15 @@ protected:
     // semaphore for access to shared frontend data
     AP_HAL::Semaphore *_sem;
 
+    // Check that the compass field is valid by using a mean filter on the vector length
+    bool field_ok(const Vector3f &field);
+    
+    uint32_t get_error_count() const { return _error_count; }
 private:
     void apply_corrections(Vector3f &mag, uint8_t i);
+    
+    // mean field length for range filter
+    float _mean_field_length;
+    // number of dropped samples. Not used for now, but can be usable to choose more reliable sensor
+    uint32_t _error_count;
 };

libraries/AP_Compass/AP_Compass_HMC5843.cpp

@@ -259,19 +259,24 @@ void AP_Compass_HMC5843::_timer()
     // correct raw_field for known errors
     correct_field(raw_field, _compass_instance);
     
-    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-        _mag_x_accum += raw_field.x;
-        _mag_y_accum += raw_field.y;
-        _mag_z_accum += raw_field.z;
-        _accum_count++;
-        if (_accum_count == 14) {
-            _mag_x_accum /= 2;
-            _mag_y_accum /= 2;
-            _mag_z_accum /= 2;
-            _accum_count = 7;
-        }
-        _sem->give();
+    if (!field_ok(raw_field)) {
+        return;
     }
+
+    if (!_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        return; 
+    }
+    _mag_x_accum += raw_field.x;
+    _mag_y_accum += raw_field.y;
+    _mag_z_accum += raw_field.z;
+    _accum_count++;
+    if (_accum_count == 14) {
+        _mag_x_accum /= 2;
+        _mag_y_accum /= 2;
+        _mag_z_accum /= 2;
+        _accum_count = 7;
+    }
+    _sem->give();
 }
 
 /*

libraries/AP_Compass/AP_Compass_MAG3110.cpp

@@ -205,52 +205,31 @@ void AP_Compass_MAG3110::_update()
 
     Vector3f raw_field = Vector3f(_mag_x, _mag_y, _mag_z) * MAG_SCALE;
 
+    // rotate raw_field from sensor frame to body frame
+    rotate_field(raw_field, _compass_instance);
+    
+    // publish raw_field (uncorrected point sample) for calibration use
+    publish_raw_field(raw_field, _compass_instance);
+    
+    // correct raw_field for known errors
+    correct_field(raw_field, _compass_instance);
 
-    bool ret=true;
-
-#if MAG3110_ENABLE_LEN_FILTER
-    float len = raw_field.length();
-    if(is_zero(compass_len)) {
-        compass_len=len;
-    } else {
-#define FILTER_KOEF 0.1
-
-        float d = abs(compass_len-len)/(compass_len+len);
-        if(d*100 > 25) { // difference more than 50% from mean value
-            printf("\ncompass len error: mean %f got %f\n", compass_len, len );
-            ret= false;
-            float k = FILTER_KOEF / (d*10); // 2.5 and more, so one bad sample never change mean more than 4%
-            compass_len = compass_len * (1-k) + len*k; // complimentary filter 1/k on bad samples
-        } else {
-            compass_len = compass_len * (1-FILTER_KOEF) + len*FILTER_KOEF; // complimentary filter 1/10 on good samples
-        }
+    if (!field_ok(raw_field)) {
+        return;
     }
-#endif
     
-    if(ret) {
-
-        // rotate raw_field from sensor frame to body frame
-        rotate_field(raw_field, _compass_instance);
-
-        // publish raw_field (uncorrected point sample) for calibration use
-        publish_raw_field(raw_field, _compass_instance);
-
-        // correct raw_field for known errors
-        correct_field(raw_field, _compass_instance);
-
-        if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
-            _mag_x_accum += raw_field.x;
-            _mag_y_accum += raw_field.y;
-            _mag_z_accum += raw_field.z;
-            _accum_count++;
-            if (_accum_count == 10) {
-                _mag_x_accum /= 2;
-                _mag_y_accum /= 2;
-                _mag_z_accum /= 2;
-                _accum_count /= 2;
-            }
-            _sem->give();
+    if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
+        _mag_x_accum += raw_field.x;
+        _mag_y_accum += raw_field.y;
+        _mag_z_accum += raw_field.z;
+        _accum_count++;
+        if (_accum_count == 10) {
+            _mag_x_accum /= 2;
+            _mag_y_accum /= 2;
+            _mag_z_accum /= 2;
+            _accum_count /= 2;
         }
+        _sem->give();
     }
 }
 

libraries/AP_Compass/AP_Compass_QMC5883L.cpp

@@ -207,14 +207,18 @@ void AP_Compass_QMC5883L::timer()
     /* correct raw_field for known errors */
     correct_field(field, _instance);
 
+    if (!field_ok(field)) {
+        return;
+    }
+
     if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
         _accum += field;
         _accum_count++;
         if(_accum_count == 20){
-        	_accum.x /= 2;
-        	_accum.y /= 2;
-        	_accum.z /= 2;
-        	_accum_count = 10;
+            _accum.x /= 2;
+            _accum.y /= 2;
+            _accum.z /= 2;
+            _accum_count = 10;
         }
         _sem->give();
     }