AP_RangeFinder: fixed filter in PulsedLightLRF

the value from the sensor can oscillate, causing it to stay locked at the last value due to the broken glitch detection. Instead use a 3 element mode filter
ArduPilot · Mar 18, 2024 · c53e8be · c53e8be
1 parent eb3af8b
commit c53e8be
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.cpp
@@ -49,6 +49,7 @@ AP_RangeFinder_PulsedLightLRF::AP_RangeFinder_PulsedLightLRF(uint8_t bus,
     : AP_RangeFinder_Backend(_state, _params)
     , _dev(hal.i2c_mgr->get_device(bus, LL40LS_ADDR))
     , rftype(_rftype)
+    , filter(1)
 {
 }
 
@@ -91,14 +92,11 @@ void AP_RangeFinder_PulsedLightLRF::timer(void)
         // read the high and low byte distance registers
         if (_dev->read_registers(LL40LS_DISTHIGH_REG | LL40LS_AUTO_INCREMENT, (uint8_t*)&val, sizeof(val))) {
             uint16_t _distance_cm = be16toh(val);
-            // remove momentary spikes
-            if (abs(_distance_cm - last_distance_cm) < 100) {
-                state.distance_m = _distance_cm * 0.01f;
-                state.last_reading_ms = AP_HAL::millis();
-                update_status();                
-            }
-            last_distance_cm = _distance_cm;
-        } else {
+            // apply mode filter
+            state.distance_m = filter.apply(_distance_cm*0.01);
+            state.last_reading_ms = AP_HAL::millis();
+            update_status();
+        } else if (AP_HAL::millis() - state.last_reading_ms > 200) {
             set_status(RangeFinder::Status::NoData);
         }
         if (!v2_hardware) {

diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h b/libraries/AP_RangeFinder/AP_RangeFinder_PulsedLightLRF.h
@@ -8,6 +8,7 @@
 #include "AP_RangeFinder_Backend.h"
 
 #include <AP_HAL/I2CDevice.h>
+#include <Filter/ModeFilter.h>
 
 /* Connection diagram
  *
@@ -57,8 +58,9 @@ class AP_RangeFinder_PulsedLightLRF : public AP_RangeFinder_Backend
     uint8_t check_reg_counter;
     bool v2_hardware;
     bool v3hp_hardware;
-    uint16_t last_distance_cm;
     RangeFinder::Type rftype;
+
+    ModeFilterFloat_Size3 filter;
 
     enum { PHASE_MEASURE, PHASE_COLLECT } phase;
 };