Fixed it;
Header file
#ifndef LandjePingSensor_h
#define LandjePingSensor_h
#include "Arduino.h"
class LandjePingSensor
{
public:
// LandjePingSensor();
Init(int pingSensorPort);
Ping();
int GetDistanceInCM();
private:
const unsigned long _maxUnsignedLong = 1 ; //pow(2,32) - 1 ; // max value unsigned long
const unsigned int _maxPingDistanceCM = 400; //maximum distance detected by ping sensor in cm
const unsigned int _minPingDistanceCM = 2 ; // minimum detectable distance
const unsigned long _maxSampleLifespan = 20000; // maximum lifespan of measurement in milliseconds
// The approximate speed of sound in dry air is given by the formula:
// c = 331.5 (m/s) + 0.6 * [air temperature in degrees Celsius]
// Give the temperatue of 21 Celcius;
// 331.5 + (0.6 * 21) = 344.1 meter/sec = 34410 cm/sec
// 34410 cm/sec = (34410 / 1000) cm/msec = 0.034410 cm/microsec
const unsigned long _speedSoundCmPerUsec = 23 ;
unsigned int _maxEchoWaitTime; // calculated max wait time in microseconds
unsigned long _echoWaitTime; // timestamp when ping echo response measurement ends
unsigned long _pingStartTime ; // stores start of ping echo reception
unsigned long _sampleTime ; // timestamp of latest sample
int _distanceInCM;
int _pingSensorPort;
uint8_t _pingPinBitmask;
volatile uint8_t *_pingOutputRegister;
volatile uint8_t *_echoInputRegister;
volatile uint8_t *_pingModeRegister;
};
#endif
Class
#include "LandjePingSensor.h"
LandjePingSensor::Init(int pingSensorPort)
{
_pingSensorPort = pingSensorPort;
_distanceInCM = 0;
_pingPinBitmask = digitalPinToBitMask(_pingSensorPort); // translate pin number to port register bitmask for selected pin.
_pingOutputRegister = portOutputRegister(digitalPinToPort(_pingSensorPort)); // Get the output (PORT) port register for selected pin.
_echoInputRegister = portInputRegister(digitalPinToPort(_pingSensorPort)); // Get the input (PIN) port register for the echo pin.
_pingModeRegister = (uint8_t *)portModeRegister(digitalPinToPort(_pingSensorPort)); // get DDR register for pin
// Sound has to travel to object and echoed back from the object.
// The maximum time to wait for an echo is roughly the maximum distance detection
// multiplied by 2 and divided by the traveling speed of sound in microseconds per cm.
_maxEchoWaitTime = (long)((_maxPingDistanceCM * 20) * _speedSoundCmPerUsec);
};
LandjePingSensor::Ping()
{
// Prevent occurence of clock overflow cycle to hang detection
if (micros() > (_maxUnsignedLong - _maxEchoWaitTime))
return; // skip detection if value of micros() may overflow
// Trigger the HC-SR04
*_pingModeRegister |= _pingPinBitmask; // Set pin to output (HIGH) in DDR register
*_pingOutputRegister &= ~_pingPinBitmask; // LOW
delayMicroseconds(2);
*_pingOutputRegister |= _pingPinBitmask; // HIGH
delayMicroseconds(10);
*_pingOutputRegister &= ~_pingPinBitmask; // LOW
// Switch port to input mode to detect echo on same pin
*_pingModeRegister &= ~_pingPinBitmask; // Set pin to input (LOW) in DDR register
// Stop measuring if another ping is in progress
if (*_echoInputRegister & _pingPinBitmask)
return; // pin still high probably other waiting for other echo.
boolean timeout = false;
_echoWaitTime = micros() + _maxEchoWaitTime; // max wait timestamp
// Wait till the ping echo returns or timeout occurs
while (!(*_echoInputRegister & _pingPinBitmask))
{ // wait till pin changes to HIGH
if (micros() > _echoWaitTime)
{
timeout = true;
break;
} // stop waiting if it takes to long based on max ping distance
}
// If ping is received, measure pulse duration
if (!timeout)
{
_echoWaitTime = micros() + _maxEchoWaitTime; // max wait timestamp
_pingStartTime = micros();
// Wait till ping echo is received
while (*_echoInputRegister & _pingPinBitmask)
{ // Wait till ping changes to LOW
if (micros() > _echoWaitTime)
{
timeout = true;
break;
} // stop waiting if it takes to long based on max ping distance
}
if (!timeout)
{
_distanceInCM = (micros() - _pingStartTime) / (_speedSoundCmPerUsec * 2);
_sampleTime = millis();
}
}
}
int LandjePingSensor::GetDistanceInCM()
{
// If recorded last sampled distance is > 2cm
if (_distanceInCM >= 2)
{
// If sample not to old
if ((_sampleTime - millis()) < _maxSampleLifespan)
return _distanceInCM; // return distance in cm
else
return 0; // return 0 distance
}
else
{
return 0; // return 0 distamce
}
}