Use median of measured values again.

Author: amandel

OpenBikeSensorFirmware/sensor.cpp

@@ -233,11 +233,12 @@ void HCSR04SensorManager::collectSensorResults() {
       dist = static_cast<uint16_t>(duration / MICRO_SEC_TO_CM_DIVIDER);
     }
     sensor->rawDistance = dist;
-    sensorValues[idx] = correctSensorOffset(dist, sensor->offset);
+    sensorValues[idx] = correctSensorOffset(medianMeasure(sensor, dist), sensor->offset);
 
 #ifdef DEVELOP
-    Serial.printf("Raw sensor[%d] distance read %03u / %03u -> *%03ucm*, duration: %zu us - echo pin state: %d\n",
-      idx, sensor->rawDistance, dist, sensorValues[idx], duration, digitalRead(sensor->echoPin));
+    Serial.printf("Raw sensor[%d] distance read %03u / %03u (%03u, %03u, %03u) -> *%03ucm*, duration: %zu us - echo pin state: %d\n",
+      idx, sensor->rawDistance, dist, sensor->distances[0], sensor->distances[1],
+      sensor->distances[2], sensorValues[idx], duration, digitalRead(sensor->echoPin));
 #endif
 
     if (sensorValues[idx] > 0 && sensorValues[idx] < sensor->minDistance)
@@ -331,6 +332,31 @@ uint32_t HCSR04SensorManager::microsSince(uint32_t a) {
   return microsBetween(micros(), a);
 }
 
+uint16_t HCSR04SensorManager::medianMeasure(HCSR04SensorInfo *const sensor, uint16_t value) {
+  sensor->distances[sensor->nextMedianDistance++] = value;
+  if (sensor->nextMedianDistance >= MEDIAN_DISTANCE_MEASURES) {
+    sensor->nextMedianDistance = 0;
+  }
+  return median(sensor->distances[0], sensor->distances[1], sensor->distances[2]);
+}
+
+uint16_t HCSR04SensorManager::median(uint16_t a, uint16_t b, uint16_t c) {
+  if (a < b) {
+    if (a >= c) {
+      return a;
+    } else if (b < c) {
+      return b;
+    }
+  } else {
+    if (a < c) {
+      return a;
+    } else if (b >= c) {
+      return b;
+    }
+  }
+  return c;
+}
+
 void IRAM_ATTR HCSR04SensorManager::isr(int idx) {
   // since the measurement of start and stop use the same interrupt
   // mechanism we should see a similar delay.

OpenBikeSensorFirmware/sensor.h

@@ -34,6 +34,8 @@ struct HCSR04SensorInfo
   uint8_t echoPin = 4;
   uint16_t offset = 0;
   uint16_t rawDistance = 0;
+  uint16_t distances[MEDIAN_DISTANCE_MEASURES] = { MAX_SENSOR_VALUE, MAX_SENSOR_VALUE, MAX_SENSOR_VALUE };
+  uint16_t nextMedianDistance = 0;
   uint16_t minDistance=MAX_SENSOR_VALUE;
   char* sensorLocation;
   unsigned long lastMinUpdate=0;
@@ -67,6 +69,8 @@ class HCSR04SensorManager
     void sendTriggerToReadySensor();
     void IRAM_ATTR isr(int idx);
     uint32_t getFixedStart(size_t idx, const HCSR04SensorInfo *sensor);
+    static uint16_t medianMeasure(HCSR04SensorInfo* const sensor, uint16_t value);
+    static uint16_t median(uint16_t a, uint16_t b, uint16_t c);
     static uint16_t correctSensorOffset(uint16_t dist, uint16_t offset);
     static boolean isReadyForStart(HCSR04SensorInfo* sensor);
     static uint32_t microsBetween(uint32_t a, uint32_t b);