Michael Mandl
4811ab8239
only emit a new distance value if echo pulse edges trigger in the right order. Fixes #1
44 lines
867 B
C++
44 lines
867 B
C++
#include "distancethread.h"
|
|
#include <wiringPi.h>
|
|
|
|
volatile unsigned int timestampHigh = 0;
|
|
volatile unsigned int pulseLength = 0;
|
|
|
|
DistanceThread::DistanceThread()
|
|
{
|
|
wiringPiSetup();
|
|
pinMode(m_triggerPin, OUTPUT);
|
|
pinMode(m_echoPin, INPUT);
|
|
}
|
|
|
|
void DistanceThread::run()
|
|
{
|
|
wiringPiISR(m_echoPin, INT_EDGE_BOTH, []
|
|
{
|
|
if (digitalRead(29) == HIGH)
|
|
{
|
|
timestampHigh = micros();
|
|
}
|
|
else if (timestampHigh != 0)
|
|
{
|
|
pulseLength = micros() - timestampHigh;
|
|
timestampHigh = 0;
|
|
}
|
|
});
|
|
|
|
while (true)
|
|
{
|
|
digitalWrite(m_triggerPin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(m_triggerPin, LOW);
|
|
delayMicroseconds(40);
|
|
|
|
if (pulseLength < 25e3)
|
|
{
|
|
emit distanceUpdated(pulseLength / 0.58);
|
|
}
|
|
|
|
delay(100);
|
|
}
|
|
}
|