From e2d48efec1001bc00fd99d2968e95d88ab75b323 Mon Sep 17 00:00:00 2001
From: Ard Kuijpers <ardkuijpers@mp.nl>
Date: Sun, 1 Mar 2020 17:14:56 +0100
Subject: [PATCH] More robust measurement

---
 src/main.cpp | 41 +++++++++++++++++++++++------------------
 1 file changed, 23 insertions(+), 18 deletions(-)

diff --git a/src/main.cpp b/src/main.cpp
index 37541bf..2a097cf 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -11,15 +11,12 @@ const float minDistance = 0.01;
 const float maxDistance = 4.0;
 const float minimumChange = 0.2;
 
-#define DISTANCE_INTERVAL 5 // s
-#define LED_INTERVAL 5 // s
+#define MEASURE_INTERVAL 500 // ms
+#define PUBLISH_INTERVAL 5000 // ms
 
 NewPing sonar(trigPin,echoPin,maxDistance*100.0);
 HomieNode obstacleNode("obstacle", "Obstacle", "object");
 
-#define heartbeatMillis 5000
-#define messageMillis 5000
-
 void signal_led(bool ons = true);
 void heartbeat_led(int times = 2);
 void longbeat_led();
@@ -34,7 +31,7 @@ void setup() {
   pinMode(ledPin, OUTPUT);
 
   Homie_setBrand("EtxeanIoT");
-  Homie_setFirmware("etxean-distancesensor", "1.0.0"); 
+  Homie_setFirmware("etxean-distancesensor", "1.0.3"); 
   Homie.setLoopFunction(loopHandler);
   obstacleNode
     .advertise("distance").setName("Distance").setDatatype("float").setUnit("m");
@@ -49,9 +46,10 @@ void loop() {
   Homie.loop();
 }
 
-long lastHeartbeat = 0;
-long lastDistanceSent = 0;
+long lastPublish = 0;
+long lastMeasurement = 0;
 float lastDistance = 0.0;
+float validDistance = -1;
 
 bool checkBounds(float value, float min, float max) {
   return !isnan(value) && value >= min &&  value <= max;
@@ -67,29 +65,36 @@ String toPayload(bool value)
 }
 
 void loopHandler() {
-  if (millis() - lastDistanceSent >= DISTANCE_INTERVAL * 1000UL || lastDistanceSent == 0) {
-    heartbeat_led();
+  if (millis() - lastMeasurement >= MEASURE_INTERVAL) {
     float distance = readDistance();
     //float distance = random(5,400)/100.0;
     Homie.getLogger() << "Distance: " << distance << " m";
-    obstacleNode.setProperty("distance").send(String(distance));
-
     bool valid = checkBounds(distance,minDistance,maxDistance);
-    Serial << ", valid = " << toPayload(valid);
+    Serial << ", valid = " << toPayload(valid) << endl;
+    if (valid)
+      validDistance = distance;
+    lastMeasurement = millis();
+  }
+  if (millis() - lastPublish >= PUBLISH_INTERVAL) {
+    heartbeat_led();
+
+    bool valid = validDistance > 0;
     obstacleNode.setProperty("valid").send(toPayload(valid));
     if (valid)
     {
-      bool changed = signalChange(distance, lastDistance);
+      Homie.getLogger() << "Publish distance: " << validDistance << " m";
+      obstacleNode.setProperty("distance").send(String(validDistance));
+      bool changed = signalChange(validDistance, lastDistance);
       obstacleNode.setProperty("changed").send(toPayload(changed));
       if (changed)
       {
         signal_led();      
-        Serial << ", changed";
+        Serial << ", changed" << endl;
       }
-      lastDistance = distance;
+      lastDistance = validDistance;
+      validDistance = -1;      
     }
-    Serial << endl;
-    lastDistanceSent = millis();
+    lastPublish = millis();
   }
 }