From a00278b8af02bb8464cb87d5a1399b339ee51aef Mon Sep 17 00:00:00 2001
From: OneSelf <36823667+ShadedSelf@users.noreply.github.com>
Date: Sun, 25 Feb 2024 19:10:46 +0100
Subject: [PATCH 1/3] Update sonar.cpp

---
 sunray/sonar.cpp | 51 +++++++++++++++++++++++++-----------------------
 1 file changed, 27 insertions(+), 24 deletions(-)

diff --git a/sunray/sonar.cpp b/sunray/sonar.cpp
index d4e089b43..dc2e0057d 100644
--- a/sunray/sonar.cpp
+++ b/sunray/sonar.cpp
@@ -20,9 +20,9 @@
 
 
 
-RunningMedian<unsigned int, 9> sonarLeftMeasurements;
-RunningMedian<unsigned int, 9> sonarRightMeasurements;
-RunningMedian<unsigned int, 9> sonarCenterMeasurements;
+RunningMedian<unsigned int, 3> sonarLeftMeasurements;
+RunningMedian<unsigned int, 3> sonarRightMeasurements;
+RunningMedian<unsigned int, 3> sonarCenterMeasurements;
 
 volatile unsigned long startTime = 0;
 volatile unsigned long echoTime = 0;
@@ -91,15 +91,35 @@ void Sonar::run() {
     distanceRight = distanceLeft = distanceCenter = 0;
     return;
   }
+
+  //sonar hit
   if (echoDuration != 0) {
     added = true;
     unsigned long raw = echoDuration;
     if (raw > MAX_DURATION) raw = MAX_DURATION;
-    if (sonarIdx == 0) sonarLeftMeasurements.add(raw);
-    else if (sonarIdx == 1) sonarCenterMeasurements.add(raw);
-    else sonarRightMeasurements.add(raw);
+    
+    //left
+    if (sonarIdx == 0) {
+      sonarLeftMeasurements.add(raw);
+      sonarLeftMeasurements.getMedian(distanceLeft);
+      distanceLeft = convertCm(distanceLeft);
+    }
+    //center
+    else if (sonarIdx == 1) {
+      sonarCenterMeasurements.add(raw);
+      sonarCenterMeasurements.getMedian(distanceCenter);
+      distanceCenter = convertCm(distanceCenter);
+    }
+    //right
+    else {
+      sonarRightMeasurements.add(raw);
+      sonarRightMeasurements.getMedian(distanceRight);
+      distanceRight = convertCm(distanceRight);
+    } 
+
     echoDuration = 0;
   }
+
   if (millis() > timeoutTime) {
     if (!added) {
       if (sonarIdx == 0) sonarLeftMeasurements.add(MAX_DURATION);
@@ -117,23 +137,6 @@ void Sonar::run() {
     timeoutTime = millis() + 50;    			 // 10
     added = false;
   }
-  if (millis() > nextEvalTime) {
-    nextEvalTime = millis() + 200;
-    float value;
-    //sonarLeftMeasurements.getLowest(distanceLeft);
-    sonarLeftMeasurements.getMedian(distanceLeft);
-    //sonar1Measurements.getAverage(avg);
-    distanceLeft = convertCm(distanceLeft);
-
-    //sonarRightMeasurements.getLowest(distanceRight);
-    sonarRightMeasurements.getMedian(distanceRight);
-    distanceRight = convertCm(distanceRight);
-
-    //sonarCenterMeasurements.getLowest(distanceCenter);
-    sonarCenterMeasurements.getMedian(distanceCenter);
-    distanceCenter = convertCm(distanceCenter);
-
-  }
 #endif
 }
 
@@ -180,7 +183,7 @@ bool Sonar::nearObstacle()
 {
 #ifdef SONAR_INSTALLED
   if (!enabled) return false;
-  int nearZone = 30; // cm
+  int nearZone = SONAR_OBSTACLE_SLOW_CM; // cm
   if ((nearObstacleTimeout != 0) && (millis() < nearObstacleTimeout)) return true;
   nearObstacleTimeout = 0;
   bool res = ((distanceLeft < triggerLeftBelow + nearZone) || (distanceCenter < triggerCenterBelow + nearZone) || (distanceRight < triggerRightBelow + nearZone));

From 9abd462f84953e3f7d0f4c2be3d551957df170a6 Mon Sep 17 00:00:00 2001
From: OneSelf <36823667+ShadedSelf@users.noreply.github.com>
Date: Sun, 25 Feb 2024 19:12:00 +0100
Subject: [PATCH 2/3] Update config_example.h

---
 sunray/config_example.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/sunray/config_example.h b/sunray/config_example.h
index 43c785c52..3dce5226b 100644
--- a/sunray/config_example.h
+++ b/sunray/config_example.h
@@ -253,6 +253,7 @@ Also, you may choose the serial port below for serial monitor output (CONSOLE).
 #define SONAR_INSTALLED 1              // uncomment if ultrasonic sensors are installed
 //#define SONAR_ENABLE true              // should ultrasonic sensor be used?
 #define SONAR_ENABLE false
+#define SONAR_OBSTACLE_SLOW_CM 30        // slow down mower below this distance from trigger (cm)
 #define SONAR_TRIGGER_OBSTACLES true     // should sonar be used to trigger obstacles? if not, mower will only slow down
 #define SONAR_LEFT_OBSTACLE_CM   10      // stop mowing operation below this distance (cm) 
 #define SONAR_CENTER_OBSTACLE_CM 10      // stop mowing operation below this distance (cm) 

From d2014bbc7d06eeaa0db9f7d582fb50389101b1f8 Mon Sep 17 00:00:00 2001
From: OneSelf <36823667+ShadedSelf@users.noreply.github.com>
Date: Mon, 8 Apr 2024 12:01:16 +0200
Subject: [PATCH 3/3] Fixes

---
 sunray/sonar.cpp | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

diff --git a/sunray/sonar.cpp b/sunray/sonar.cpp
index dc2e0057d..4d38d52e6 100644
--- a/sunray/sonar.cpp
+++ b/sunray/sonar.cpp
@@ -122,9 +122,22 @@ void Sonar::run() {
 
   if (millis() > timeoutTime) {
     if (!added) {
-      if (sonarIdx == 0) sonarLeftMeasurements.add(MAX_DURATION);
-      else if (sonarIdx == 1) sonarCenterMeasurements.add(MAX_DURATION);
-      else sonarRightMeasurements.add(MAX_DURATION);
+      unsigned int td;
+      if (sonarIdx == 0) {
+        sonarLeftMeasurements.add(MAX_DURATION);
+        sonarLeftMeasurements.getMedian(td);
+        distanceLeft = convertCm(td);
+      }
+      else if (sonarIdx == 1) {
+        sonarCenterMeasurements.add(MAX_DURATION);
+        sonarCenterMeasurements.getMedian(td);
+        distanceCenter = convertCm(td);
+      }
+      else if (sonarIdx == 2) {
+        sonarRightMeasurements.add(MAX_DURATION);
+        sonarRightMeasurements.getMedian(td);
+        distanceRight = convertCm(td);
+      }
     }
     //if (millis() > nextSonarTime){
     sonarIdx = (sonarIdx + 1) % 3;