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 "common/time.h"
  29
  30 #include "drivers/exti.h"
  31 #include "drivers/io.h"
  32 #include "drivers/nvic.h"
  33 #include "drivers/rcc.h"
  34 #include "drivers/time.h"
  35
  36 #include "drivers/rangefinder/rangefinder.h"
  37
  38 #include "rangefinder_hcsr04.h"
  39
  40 #define HCSR04_MAX_RANGE_CM 400 // 4m, from HC-SR04 spec sheet
  41 #define HCSR04_DETECTION_CONE_DECIDEGREES 300 // recommended cone angle30 degrees, from HC-SR04 spec sheet
  42 #define HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES 450 // in practice 45 degrees seems to work well
  43
  44 /* HC-SR04 consists of ultrasonic transmitter, receiver, and control circuits.
  45  * When triggered it sends out a series of 40KHz ultrasonic pulses and receives
  46  * echo from an object. The distance between the unit and the object is calculated
  47  * by measuring the traveling time of sound and output it as the width of a TTL pulse.
  48  *
  49  * *** Warning: HC-SR04 operates at +5V ***
  50  *
  51  */
  52
  53 static volatile timeDelta_t hcsr04SonarPulseTravelTime = 0;
  54 static volatile timeMs_t lastMeasurementReceivedAt;
  55 static int32_t lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
  56 static timeMs_t lastMeasurementStartedAt = 0;
  57
  58 static extiCallbackRec_t hcsr04_extiCallbackRec;
  59
  60 static IO_t echoIO;
  61 static IO_t triggerIO;
  62
  63 #if !defined(UNIT_TEST)
  64 static void hcsr04_extiHandler(extiCallbackRec_t* cb)
  65 {
  66     UNUSED(cb);
  67
  68     static timeUs_t timing_start;
  69
  70     const timeUs_t currentTimeUs = microsISR();
  71
  72     if (IORead(echoIO) != 0) {
  73         timing_start = currentTimeUs;
  74     } else {
  75         const timeUs_t timing_stop = currentTimeUs;
  76         if (timing_stop > timing_start) {
  77             lastMeasurementReceivedAt = currentTimeUs / 1000;
  78             hcsr04SonarPulseTravelTime = timing_stop - timing_start;
  79         }
  80     }
  81 }
  82 #endif
  83
  84 static void hcsr04_init(rangefinderDev_t *dev)
  85 {
  86     UNUSED(dev);
  87 }
  88
  89 #define HCSR04_MINIMUM_FIRING_INTERVAL_MS 60
  90
  91 /*
  92  * Start a range reading
  93  * Called periodically by the scheduler
  94  * Measurement reading is done asynchronously, using interrupt
  95  */
  96 static  void hcsr04_start_reading(void)
  97 {
  98 #if !defined(UNIT_TEST)
  99     IOHi(triggerIO);
 100     delayMicroseconds(11);
 101     IOLo(triggerIO);
 102 #endif
 103 }
 104
 105 static void hcsr04_update(rangefinderDev_t *dev)
 106 {
 107     UNUSED(dev);
 108
 109     const timeMs_t timeNowMs = millis();
 110
 111     // We should have a valid measurement within 60ms of trigger
 112     if (lastMeasurementReceivedAt > lastMeasurementStartedAt && (lastMeasurementReceivedAt - lastMeasurementStartedAt) <= HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
 113         // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
 114         // The ping travels out and back, so to find the distance of the
 115         // object we take half of the distance traveled.
 116         // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
 117
 118         lastCalculatedDistance = hcsr04SonarPulseTravelTime / 59;
 119         if (lastCalculatedDistance > HCSR04_MAX_RANGE_CM) {
 120             lastCalculatedDistance = RANGEFINDER_OUT_OF_RANGE;
 121         }
 122     } else if ((lastMeasurementReceivedAt - lastMeasurementStartedAt) > HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
 123         // No measurement within reasonable time - indicate failure
 124         lastCalculatedDistance = RANGEFINDER_HARDWARE_FAILURE;
 125     }
 126
 127     // the firing interval of the trigger signal should be greater than 60ms
 128     // to avoid interference between consecutive measurements
 129     if (timeNowMs > lastMeasurementStartedAt + HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
 130         // Trigger a new measurement
 131         lastMeasurementStartedAt = timeNowMs;
 132         hcsr04_start_reading();
 133     }
 134 }
 135
 136 /**
 137  * Get the distance that was measured by the last pulse, in centimeters.
 138  */
 139 static int32_t hcsr04_get_distance(rangefinderDev_t *dev)
 140 {
 141     UNUSED(dev);
 142
 143     const int32_t result = lastCalculatedDistance;
 144
 145     lastCalculatedDistance = RANGEFINDER_NO_NEW_DATA;
 146
 147     return result;
 148 }
 149
 150 bool hcsr04Detect(rangefinderDev_t *dev, const sonarConfig_t * rangefinderHardwarePins)
 151 {
 152     bool detected = false;
 153
 154     triggerIO = IOGetByTag(rangefinderHardwarePins->triggerTag);
 155     echoIO = IOGetByTag(rangefinderHardwarePins->echoTag);
 156
 157     if (IOGetOwner(triggerIO) || IOGetOwner(echoIO)) {
 158         return false;
 159     }
 160
 161     // trigger pin
 162     IOInit(triggerIO, OWNER_SONAR_TRIGGER, 0);
 163     IOConfigGPIO(triggerIO, IOCFG_OUT_PP);
 164     IOLo(triggerIO);
 165     delay(100);
 166
 167     // echo pin
 168     IOInit(echoIO, OWNER_SONAR_ECHO, 0);
 169     IOConfigGPIO(echoIO, IOCFG_IN_FLOATING);
 170
 171     // HC-SR04 echo line should be low by default and should return a response pulse when triggered
 172     if (IORead(echoIO) == false) {
 173         for (int i = 0; i < 5 && !detected; i++) {
 174             timeMs_t requestTime = millis();
 175             hcsr04_start_reading();
 176
 177             while ((millis() - requestTime) < HCSR04_MINIMUM_FIRING_INTERVAL_MS) {
 178                 if (IORead(echoIO) == true) {
 179                     detected = true;
 180                     break;
 181                 }
 182             }
 183         }
 184     }
 185
 186     if (detected) {
 187         // Hardware detected - configure the driver
 188         EXTIHandlerInit(&hcsr04_extiCallbackRec, hcsr04_extiHandler);
 189         EXTIConfig(echoIO, &hcsr04_extiCallbackRec, NVIC_PRIO_SONAR_EXTI, IOCFG_IN_FLOATING, BETAFLIGHT_EXTI_TRIGGER_BOTH); // TODO - priority!
 190         EXTIEnable(echoIO);
 191
 192         dev->delayMs = 100;
 193         dev->maxRangeCm = HCSR04_MAX_RANGE_CM;
 194         dev->detectionConeDeciDegrees = HCSR04_DETECTION_CONE_DECIDEGREES;
 195         dev->detectionConeExtendedDeciDegrees = HCSR04_DETECTION_CONE_EXTENDED_DECIDEGREES;
 196
 197         dev->init = &hcsr04_init;
 198         dev->update = &hcsr04_update;
 199         dev->read = &hcsr04_get_distance;
 200
 201         return true;
 202     }
 203     else {
 204         // Not detected - free resources
 205         IORelease(triggerIO);
 206         IORelease(echoIO);
 207         return false;
 208     }
 209 }
 210 #endif