src/main/drivers/rangefinder/rangefinder_hcsr04.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23
  24 #include <platform.h>
  25
  26 #if defined(USE_RANGEFINDER_HCSR04)
  27
  28 #include "build/build_config.h"
  29 #include "build/debug.h"
  30
  31 #include "common/time.h"
  32
  33 #include "drivers/time.h"
  34 #include "drivers/exti.h"
  35 #include "drivers/io.h"
  36 #include "drivers/nvic.h"
  37 #include "drivers/rcc.h"
  38
  39 #include "drivers/rangefinder/rangefinder.h"
  40 #include "drivers/rangefinder/rangefinder_hcsr04.h"
  41
  42 #define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
  43 #define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
  44 #define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
  45
  46
  47 /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
  48  * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
  49  * echo from an object. The distance between the unit and the object is calculated
  50  * by measuring the traveling time of sound and output it as the width of a TTL pulse.
  51  *
  52  * *** Warning: HC-SR04 operates at +5V ***
  53  *
  54  */
  55
  56 static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
  57 static volatile timeMs_t lastMeasurementReceivedAt;
  58 static volatile int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
  59 static timeMs_t lastMeasurementStartedAt = 0;
  60
  61 #ifdef USE_EXTI
  62 static extiCallbackRec_t hcsr04_extiCallbackRec;
  63 #endif
  64
  65 static IO_t echoIO;
  66 static IO_t triggerIO;
  67
  68 #if !defined(UNIT_TEST)
  69 void hcsr04_extiHandler(extiCallbackRec_t* cb)
  70 {
  71     static timeUs_t timing_start;
  72     UNUSED(cb);
  73
  74     if (IORead(echoIO) != 0) {
  75         timing_start = micros();
  76     } else {
  77         const timeUs_t timing_stop = micros();
  78         if (timing_stop > timing_start) {
  79             lastMeasurementReceivedAt = millis();
  80             hcsr04SonarPulseTravelTime = timing_stop - timing_start;
  81         }
  82     }
  83 }
  84 #endif
  85
  86 void hcsr04_init(rangefinderDev_t *dev)
  87 {
  88     UNUSED(dev);
  89 }
  90
  91 #define HCSR04_MinimumFiringIntervalMs 60
  92
  93 /*
  94  * Start a range reading
  95  * Called periodically by the scheduler
  96  * Measurement reading is done asynchronously, using interrupt
  97  */
  98 void hcsr04_start_reading(void)
  99 {
 100 #if !defined(UNIT_TEST)
 101 #ifdef RANGEFINDER_HCSR04_TRIG_INVERTED
 102     IOLo(triggerIO);
 103     delayMicroseconds(11);
 104     IOHi(triggerIO);
 105 #else
 106     IOHi(triggerIO);
 107     delayMicroseconds(11);
 108     IOLo(triggerIO);
 109 #endif
 110 #endif
 111 }
 112
 113 void hcsr04_update(rangefinderDev_t *dev)
 114 {
 115     UNUSED(dev);
 116     const timeMs_t timeNowMs = millis();
 117
 118     // the firing interval of the trigger signal should be greater than 60ms
 119     // to avoid interference between consecutive measurements
 120     if (timeNowMs > lastMeasurementStartedAt + HCSR04_MinimumFiringIntervalMs) {
 121         // We should have a valid measurement within 60ms of trigger
 122         if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MinimumFiringIntervalMs) {
 123             // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
 124             // The ping travels out and back, so to find the distance of the
 125             // object we take half of the distance traveled.
 126             // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
 127
 128             lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
 129             if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
 130                 lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
 131             }
 132         }
 133         else {
 134             // No measurement within reasonable time - indicate failure
 135             lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
 136         }
 137
 138         // Trigger a new measurement
 139         lastMeasurementStartedAt = timeNowMs;
 140         hcsr04_start_reading();
 141     }
 142 }
 143
 144 /**
 145  * Get the distance that was measured by the last pulse, in centimeters.
 146  */
 147 int32_t hcsr04_get_distance(rangefinderDev_t *dev)
 148 {
 149     UNUSED(dev);
 150     return lastCalculatedDistance;
 151 }
 152
 153 bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
 154 {
 155     bool detected = false;
 156
 157 #ifdef STM32F10X
 158     // enable AFIO for EXTI support
 159     RCC_ClockCmd(RCC_APB2(AFIO), ENABLE);
 160 #endif
 161
 162 #if defined(STM32F3) || defined(STM32F4)
 163     RCC_ClockCmd(RCC_APB2(SYSCFG), ENABLE); // XXX Do we need this?
 164 #endif
 165
 166     triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
 167     echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
 168
 169     if (IOGetOwner(triggerIO) != OWNER_FREE) {
 170         return false;
 171     }
 172
 173     if (IOGetOwner(echoIO) != OWNER_FREE) {
 174         return false;
 175     }
 176
 177     // trigger pin
 178     IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
 179     IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
 180     IOLo(triggerIO);
 181     delay(100);
 182
 183     // echo pin
 184     IOInit(echoIO, OWNER_SONAR_ECHO, 0);
 185     IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
 186
 187     // HC-SR04 echo line should be low by default and should return a response pulse when triggered
 188     if (IORead(echoIO) == false) {
 189         for (int i = 0; i < 5 && !detected; i++) {
 190             timeMs_t requestTime = millis();
 191             hcsr04_start_reading();
 192
 193             while ((millis() - requestTime) < HCSR04_MinimumFiringIntervalMs) {
 194                 if (IORead(echoIO) == true) {
 195                     detected = true;
 196                     break;
 197                 }
 198             }
 199         }
 200     }
 201
 202     if (detected) {
 203         // Hardware detected - configure the driver
 204 #ifdef USE_EXTI
 205         EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
 206         EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, EXTI_Trigger_Rising_Falling); // TODO - priority!
 207         EXTIEnable(echoIO, true);
 208 #endif
 209
 210         dev->delayMs = 100;
 211         dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
 212         dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
 213         dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
 214
 215         dev->init = &hcsr04_init;
 216         dev->update = &hcsr04_update;
 217         dev->read = &hcsr04_get_distance;
 218
 219         return true;
 220     }
 221     else {
 222         // Not detected - free resources
 223         IORelease(triggerIO);
 224         IORelease(echoIO);
 225         return false;
 226     }
 227 }
 228
 229 #endif