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