qEcho/source/hcsr04.cpp

59 lines
963 B
C++
Raw Permalink Normal View History

#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;
}