147 lines
3.3 KiB
C++
147 lines
3.3 KiB
C++
#include "Sensors/HCSR04.h"
|
|
|
|
HCSR04::HCSR04(int triggerPin, int echoPin)
|
|
{
|
|
_triggerPin = triggerPin;
|
|
_echoPin = echoPin;
|
|
_temperature = HCSR04_INIT_TEMPERATURE;
|
|
_humidity = HCSR04_INIT_HUMIDITY;
|
|
}
|
|
|
|
void HCSR04::begin()
|
|
{
|
|
pinMode(_triggerPin, OUTPUT);
|
|
pinMode(_echoPin, INPUT);
|
|
}
|
|
|
|
void HCSR04::setTemperature(int temperature)
|
|
{
|
|
_temperature = temperature;
|
|
}
|
|
|
|
void HCSR04::setHumidity(int humidity)
|
|
{
|
|
_humidity = humidity;
|
|
}
|
|
|
|
/**
|
|
* Get a distance reading.
|
|
* When reading the distance fails, -1 is returned.
|
|
* Otherwise the distance in mm.
|
|
*/
|
|
int HCSR04::readDistance()
|
|
{
|
|
_setSpeedOfSound();
|
|
_setEchoTimeout();
|
|
_takeSamples();
|
|
if (_haveEnoughSamples())
|
|
{
|
|
_sortSamples();
|
|
return _computeAverage();
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
/**
|
|
* Sets the speed of sound in mm/Ms, depending on the temperature
|
|
* and relative humidity. I derived this formula from a YouTube
|
|
* video about the HC-SR04: https://youtu.be/6F1B_N6LuKw?t=1548
|
|
*/
|
|
void HCSR04::_setSpeedOfSound()
|
|
{
|
|
_speedOfSound =
|
|
0.3314 +
|
|
(0.000606 * _temperature) +
|
|
(0.0000124 * _humidity);
|
|
}
|
|
|
|
void HCSR04::_setEchoTimeout()
|
|
{
|
|
_echoTimeout = HCSR04_MAX_MM * 2 / _speedOfSound;
|
|
}
|
|
|
|
void HCSR04::_takeSamples()
|
|
{
|
|
_successfulSamples = 0;
|
|
for (int i = 0; i < HCSR04_SAMPLES_TAKE; i++)
|
|
{
|
|
// Because I notice some repeating patterns in timings when doing
|
|
// a tight loop here, I add some random waits to get a better spread
|
|
// of sample values.
|
|
if (i > 0)
|
|
{
|
|
delay(HCSR04_SAMPLE_WAIT + random(HCSR04_SAMPLE_WAIT_SPREAD));
|
|
}
|
|
int distance = _takeSample();
|
|
if (distance != -1)
|
|
{
|
|
_samples[i] = distance;
|
|
_successfulSamples++;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool HCSR04::_haveEnoughSamples()
|
|
{
|
|
return _successfulSamples >= HCSR04_SAMPLES_USE;
|
|
}
|
|
|
|
int HCSR04::_takeSample()
|
|
{
|
|
// Send 10μs trigger to ask sensor for a measurement.
|
|
digitalWrite(HCSR04_TRIG_PIN, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(HCSR04_TRIG_PIN, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(HCSR04_TRIG_PIN, LOW);
|
|
|
|
// Measure the length of echo signal.
|
|
unsigned long durationMicroSec = pulseIn(HCSR04_ECHO_PIN, HIGH, _echoTimeout);
|
|
|
|
// Compute the distance, based on the echo signal length.
|
|
double distance = durationMicroSec / 2.0 * _speedOfSound;
|
|
if (distance < HCSR04_MIN_MM || distance >= HCSR04_MAX_MM)
|
|
{
|
|
return -1;
|
|
}
|
|
else
|
|
{
|
|
return distance;
|
|
}
|
|
}
|
|
|
|
void HCSR04::_sortSamples()
|
|
{
|
|
int holder, x, y;
|
|
for (x = 0; x < _successfulSamples; x++)
|
|
{
|
|
for (y = 0; y < _successfulSamples - 1; y++)
|
|
{
|
|
if (_samples[y] > _samples[y + 1])
|
|
{
|
|
holder = _samples[y + 1];
|
|
_samples[y + 1] = _samples[y];
|
|
_samples[y] = holder;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Compute the average of the samples. To get rid of measuring extremes,
|
|
* only a subset of measurements from the middle are used.
|
|
* When not enough samples were collected in the previous steps, then
|
|
* NAN is returned.
|
|
*/
|
|
int HCSR04::_computeAverage()
|
|
{
|
|
float sum = 0;
|
|
int offset = (_successfulSamples - HCSR04_SAMPLES_USE) / 2;
|
|
for (int i = 0; i < HCSR04_SAMPLES_USE; i++)
|
|
{
|
|
sum += _samples[i + offset];
|
|
}
|
|
|
|
return round(sum / HCSR04_SAMPLES_USE);
|
|
}
|