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