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) 
diff --git a/sunray/sonar.cpp b/sunray/sonar.cpp
index d4e089b43..4d38d52e6 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,20 +91,53 @@ 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);
-      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;
@@ -117,23 +150,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 +196,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));