From 142efdfe0014b552e6c01bd768243f7a8c257897 Mon Sep 17 00:00:00 2001 From: Michael Mandl Date: Mon, 16 Oct 2017 18:08:44 +0200 Subject: [PATCH] Register interrupt handler only once. Fixes #9 --- source/distancethread.cpp | 4 ++- source/hcsr04.cpp | 58 +++++++++++++++++++++++++++++++++++++++ source/hcsr04.h | 17 ++++++++++++ source/median.h | 26 ++++++++++++++++++ 4 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 source/hcsr04.cpp create mode 100644 source/hcsr04.h create mode 100644 source/median.h 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 + +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 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 +#include + +template +double median(std::vector 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; + } +}