diff --git a/source/distancethread.cpp b/source/distancethread.cpp
index 37867b4..b2df07d 100644
--- a/source/distancethread.cpp
+++ b/source/distancethread.cpp
@@ -7,7 +7,9 @@ DistanceThread::DistanceThread(unsigned int triggerPin, unsigned int echoPin)
 
 void DistanceThread::run()
 {
-    while (true)
+	m_hcsr04.run();
+
+	while (true)
     {
 		emit distanceUpdated(m_hcsr04.getDistance());
 		msleep(100);
diff --git a/source/hcsr04.cpp b/source/hcsr04.cpp
new file mode 100644
index 0000000..9f71270
--- /dev/null
+++ b/source/hcsr04.cpp
@@ -0,0 +1,58 @@
+#include "hcsr04.h"
+#include "median.h"
+#include <wiringPi.h>
+
+volatile unsigned int timestampHigh = 0;
+volatile unsigned int pulseLength = 0;
+
+HCSR04::HCSR04(unsigned int triggerPin, unsigned int echoPin)
+	: m_triggerPin(triggerPin)
+	, m_echoPin(echoPin)
+{
+	wiringPiSetup();
+	pinMode(m_triggerPin, OUTPUT);
+	pinMode(m_echoPin, INPUT);
+}
+
+void HCSR04::run()
+{
+	wiringPiISR(m_echoPin, INT_EDGE_BOTH, []
+	{
+		if (digitalRead(29) == HIGH)
+		{
+			timestampHigh = micros();
+		}
+		else if (timestampHigh != 0)
+		{
+			pulseLength = micros() - timestampHigh;
+			timestampHigh = 0;
+		}
+	});
+}
+
+double HCSR04::getDistance() 
+{
+	std::vector<double> values;
+	for (int i = 0; i < 15; ++i)
+	{
+		digitalWrite(m_triggerPin, HIGH);
+		delayMicroseconds(10);
+		digitalWrite(m_triggerPin, LOW);
+		delayMicroseconds(40);
+
+		if (pulseLength < 25e3)
+		{
+			values.push_back(pulseLength);	
+		}
+
+		delay(10);
+	}
+
+	if (!values.empty())
+	{
+		return median(values) / 0.58;
+	}
+
+	return 0;
+}
+
diff --git a/source/hcsr04.h b/source/hcsr04.h
new file mode 100644
index 0000000..2badc9d
--- /dev/null
+++ b/source/hcsr04.h
@@ -0,0 +1,17 @@
+#pragma once
+
+class HCSR04
+{
+private:
+	unsigned int m_triggerPin;
+	unsigned int m_echoPin;
+
+public:
+	HCSR04() = delete;
+	HCSR04(unsigned int triggerPin, unsigned int echoPin);
+
+	void run();
+
+	double getDistance();
+};
+
diff --git a/source/median.h b/source/median.h
new file mode 100644
index 0000000..24443f1
--- /dev/null
+++ b/source/median.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <vector>
+#include <algorithm>
+
+template<typename VALUE_TYPE>
+double median(std::vector<VALUE_TYPE> values) 
+{
+	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;
+	}
+}