 void loop() {
   unsigned long time = millis();
 
-  if (time - lastCheck > 2500) {
-    lastLedState = !lastLedState;
-    
+  if (time - lastCheck > 100) {
+    lastLedState = !lastLedState;    
     digitalWrite(PIN_WORKING_LED, lastLedState ? HIGH : LOW);
 
-    int cm = sonar.ping_cm();
-    
-    if (cm > 0) {
-      if (abs(lastDistance - cm) > 1) {
-        lastDistance = cm;
-
-#ifdef DEBUG_ENABLED       
-        Serial.print("Ping: ");
-        Serial.print(cm);
-        Serial.println("cm");
-#endif        
-
-        if (settings.floorLevel > 0 && settings.safeDepth > 0) {
-          int depth = settings.floorLevel - cm;
-
-          if (depth <= settings.warnDepth) {
-            smartthing.shieldSetLED(0, 4, 0);
-          } else {
-            if (depth <= settings.safeDepth) {
-              smartthing.shieldSetLED(6, 4, 0); 
+    measurements.push(sonar.ping());    
+
+    if (measurements.valid()) 
+    {
+      int cm = round((float)measurements.average() / US_ROUNDTRIP_CM);
+      
+#ifdef DEBUG_RAW_ENABLED
+      Serial.print("Raw: ");
+      Serial.print((float)measurements.average() / US_ROUNDTRIP_CM);
+      Serial.print("  ::  ");
+      for (int i = 0; i < measurements.size(); ++i) 
+      {
+        Serial.print(measurements._sorted[i]);
+        Serial.print(" ");
+      }
+      Serial.println("");
+#endif  
+
+      if (time - lastUpdate > 2500) 
+      {
+        if (lastDistance != cm) {
+          lastDistance = cm;
+  
+  #ifdef DEBUG_ENABLED       
+          Serial.print("Ping: ");
+          Serial.print(cm);
+          Serial.println("cm");
+  #endif        
+  
+          if (settings.floorLevel > 0 && settings.safeDepth > 0) {
+            int depth = settings.floorLevel - cm;
+  
+            if (depth <= settings.warnDepth) {
+              smartthing.shieldSetLED(0, 4, 0);
             } else {
-              smartthing.shieldSetLED(8, 0, 0);               
+              if (depth <= settings.safeDepth) {
+                smartthing.shieldSetLED(6, 4, 0); 
+              } else {
+                smartthing.shieldSetLED(8, 0, 0);               
+              }
             }
           }
-        }
-
-        snprintf(message, sizeof(message), "level:%d", cm);
-        smartthing.send(message);
+  
+          snprintf(message, sizeof(message), "level:%d", cm);
+          smartthing.send(message);
+  
+          lastUpdate = time;
+        }        
       }
     }
     

  0
};

int lastDistance = 0;
int lastLedState = 0;
unsigned long lastCheck = 0;

char message[MESSAGE_BUF];


#include "SmartThings.h"

#define DEBUG_ENABLED      1
// #define DEBUG_RAW_ENABLED  1

#define PIN_THING_RX       3
#define PIN_THING_TX       2
