filter noisy measurements

Author: gluap

src/sensor.cpp

@@ -320,8 +320,10 @@ bool HCSR04SensorManager::collectSensorResult(uint8_t sensorId) {
   }
   sensor->rawDistance = dist;
   sensor->median->addValue(dist);
-  sensorValues[sensorId] =
-    sensor->distance = correctSensorOffset(medianMeasure(sensor, dist), sensor->offset);
+  uint16_t distance = medianMeasure(sensor, dist);
+  if (MAX_SAMPLE_DISTANCE_DEVIATION > range(sensor->distances, MEDIAN_DISTANCE_MEASURES))
+    sensorValues[sensorId] =
+      sensor->distance = correctSensorOffset(distance, sensor->offset);
 
   log_v("Raw sensor[%d] distance read %03u / %03u (%03u, %03u, %03u) -> *%03ucm*, duration: %zu us - echo pin state: %d",
     sensorId, sensor->rawDistance, dist, sensor->distances[0], sensor->distances[1],
@@ -472,3 +474,18 @@ uint16_t HCSR04SensorManager::median(uint16_t a, uint16_t b, uint16_t c) {
   }
   return c;
 }
+
+
+uint16_t HCSR04SensorManager::range(uint16_t values[], uint16_t size) {
+  uint16_t minval = values[0];
+  uint16_t maxval = values[0];
+  for (uint16_t i = 1; i<size; ++i) {
+    if (values[i]<minval) {
+      minval = values[i];
+    }
+    if (values[i] > maxval) {
+      maxval = values[i];
+    }
+  }
+  return maxval - minval;
+}

src/sensor.h

@@ -53,6 +53,7 @@ const uint32_t MICRO_SEC_TO_CM_DIVIDER = 58; // sound speed 340M/S, 2 times back
 
 
 const uint16_t MEDIAN_DISTANCE_MEASURES = 3;
+const uint16_t MAX_SAMPLE_DISTANCE_DEVIATION = 10;
 const uint16_t MAX_NUMBER_MEASUREMENTS_PER_INTERVAL = 35; //  is 1000/SENSOR_QUIET_PERIOD_AFTER_START_MICRO_SEC
 extern const uint16_t MAX_SENSOR_VALUE;
 
@@ -132,6 +133,7 @@ class HCSR04SensorManager {
     static uint32_t microsBetween(uint32_t a, uint32_t b);
     static uint32_t microsSince(uint32_t a);
     static uint16_t millisSince(uint16_t milliseconds);
+    static uint16_t range(uint16_t values[], uint16_t size);
     static void updateStatistics(HCSR04SensorInfo * const sensor);
     uint16_t startReadingMilliseconds = 0;
     uint8_t primarySensor = 1;