From 140ade4ef678f02067427af27f22265e2fbd5e4f Mon Sep 17 00:00:00 2001
From: Michael Mandl <mandlm@molez.net>
Date: Mon, 16 Oct 2017 08:04:08 +0200
Subject: [PATCH] Implemented median for distance measurement, fixes #3

---
 source/distancethread.cpp | 51 ++++++++++++++++++++++++++++++++-------
 source/distancethread.h   |  5 ++++
 2 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/source/distancethread.cpp b/source/distancethread.cpp
index 2cabf73..fecd077 100644
--- a/source/distancethread.cpp
+++ b/source/distancethread.cpp
@@ -28,16 +28,49 @@ void DistanceThread::run()
 
     while (true)
     {
-        digitalWrite(m_triggerPin, HIGH);
-        delayMicroseconds(10);
-        digitalWrite(m_triggerPin, LOW);
-        delayMicroseconds(40);
+		std::vector<double> values;
+		for (int i = 0; i < 5; ++i)
+		{
+	        digitalWrite(m_triggerPin, HIGH);
+    	    delayMicroseconds(10);
+        	digitalWrite(m_triggerPin, LOW);
+	        delayMicroseconds(40);
 
-        if (pulseLength < 25e3)
-        {
-            emit distanceUpdated(pulseLength / 0.58);
-        }
+    	    if (pulseLength < 25e3)
+        	{
+            	values.push_back(pulseLength);	
+	        }
+	  
+			delay(10);
+		}
 
-        delay(100);
+		if (!values.empty())
+		{
+			emit distanceUpdated(median(values) / 0.58);
+		}
+
+		delay(100);
     }
 }
+
+template<typename VALUE_TYPE>
+double DistanceThread::median(std::vector<VALUE_TYPE> values) const
+{
+	if (values.size() == 0)
+	{
+		return 0.0;
+	}
+
+	size_t centerIndex = values.size() / 2;
+	std::nth_element(values.begin(), values.begin() + centerIndex, values.end());
+	
+	if (values.size() % 2 == 1)
+	{
+		return values[centerIndex];
+	}
+	else
+	{
+		std::nth_element(values.begin(), values.begin() + centerIndex + 1, values.end());
+		return (values[centerIndex] + values[centerIndex + 1]) / 2.0;
+	}
+}
diff --git a/source/distancethread.h b/source/distancethread.h
index 4429153..f70bf0b 100644
--- a/source/distancethread.h
+++ b/source/distancethread.h
@@ -1,5 +1,6 @@
 #pragma once
 
+#include <vector>
 #include <QThread>
 
 class DistanceThread : public QThread
@@ -16,6 +17,10 @@ public:
 protected:
     void run() override;
 
+private:
+	template<typename VALUE_TYPE>
+	double median(std::vector<VALUE_TYPE> values) const;
+
 signals:
     void distanceUpdated(double distMeters);
 };