void hcsr04_update(rangefinderDev_t *dev)
{
UNUSED(dev);
const timeMs_t timeNowMs = millis();
// the firing interval of the trigger signal should be greater than 60ms
// to avoid interference between consecutive measurements
if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
// We should have a valid measurement within 60ms of trigger
if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the
// object we take half of the distance traveled.
// 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
}
}
else {
// No measurement within reasonable time - indicate failure
lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
}
// Trigger a new measurement
lastMeasurementStartedAt = timeNowMs;
hcsr04_start_reading();
}
}
void sonarUpdate(void)
{
hcsr04_start_reading();
}
void sonarUpdate(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
hcsr04_start_reading();
}
bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
{
bool detected = false;
#ifdef STM32F10X
// enable AFIO for EXTI support
RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
#endif
#if defined(STM32F3) || defined(STM32F4)
RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
#endif
triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
if (IOGetOwner(triggerIO) != OWNER_FREE) {
return false;
}
if (IOGetOwner(echoIO) != OWNER_FREE) {
return false;
}
// trigger pin
IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
IOLo(triggerIO);
delay(100);
// echo pin
IOInit(echoIO, OWNER_SONAR_ECHO, 0);
IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
// HC-SR04 echo line should be low by default and should return a response pulse when triggered
if (IORead(echoIO) == false) {
for (int i = 0; i < 5 && !detected; i++) {
timeMs_t requestTime = millis();
hcsr04_start_reading();
while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
if (IORead(echoIO) == true) {
detected = true;
break;
}
}
}
}
if (detected) {
// Hardware detected - configure the driver
#ifdef USE_EXTI
EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
EXTIEnable(echoIO, true);
#endif
dev->delayMs = 100;
dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
dev->init = &hcsr04_init;
dev->update = &hcsr04_update;
dev->read = &hcsr04_get_distance;
return true;
}
else {
// Not detected - free resources
IORelease(triggerIO);
IORelease(echoIO);
return false;
}
}
