src/main/drivers/rangefinder/rangefinder_us42.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20
  21 #include "platform.h"
  22
  23 #if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_US42)
  24
  25 #include "build/build_config.h"
  26
  27 #include "drivers/time.h"
  28 #include "drivers/bus_i2c.h"
  29
  30 #include "drivers/rangefinder/rangefinder.h"
  31 #include "drivers/rangefinder/rangefinder_us42.h"
  32
  33 #include "build/debug.h"
  34
  35 // GY-US42(v2) Ultrasonic Range Sensor
  36 #define US42_MAX_RANGE_CM 400 // vcc=3.3v -> 500cm; vcc=5v -> 700cm; Ardupilot recommends a maximum of 400cm
  37 #define US42_DETECTION_CONE_DECIDEGREES 250
  38 #define US42_DETECTION_CONE_EXTENDED_DECIDEGREES 300
  39 #define US42_MIN_PROBE_INTERVAL 50
  40
  41 #define US42_I2C_ADDRESS 0x70
  42 #define US42_I2C_REGISTRY_BASE 0x00
  43 #define US42_I2C_REGISTRY_PROBE 0x51
  44 #define US42_I2C_REGISTRY_STATUS_OK 0x00
  45 #define US42_I2C_REGISTRY_DISTANCE_HIGH 0x00
  46 #define US42_I2C_REGISTRY_DISTANCE_LOW 0x01
  47
  48 volatile int32_t us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
  49 static int16_t minimumReadingIntervalMs = US42_MIN_PROBE_INTERVAL;
  50 static uint32_t timeOfLastMeasurementMs;
  51 uint8_t nullProbeCommandValue[0];
  52 static bool isUs42Responding = false;
  53
  54 static void us42Init(rangefinderDev_t *rangefinder)
  55 {
  56     busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
  57 }
  58
  59 void us42Update(rangefinderDev_t *rangefinder)
  60 {
  61     uint8_t data[2];
  62     isUs42Responding = busReadBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, data, 2);
  63
  64     if (isUs42Responding) {
  65         us42MeasurementCm = (int32_t)data[0] << 8 | (int32_t)data[1];
  66
  67         if (us42MeasurementCm > US42_MAX_RANGE_CM) {
  68              us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
  69         }
  70
  71     } else {
  72         us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
  73     }
  74
  75     const timeMs_t timeNowMs = millis();
  76     if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
  77         // measurement repeat interval should be greater than minimumReadingIntervalMs
  78         // to avoid interference between connective measurements.
  79         timeOfLastMeasurementMs = timeNowMs;
  80         busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0);
  81     }
  82 }
  83
  84 /**
  85  * Get the distance that was measured by the last pulse, in centimeters.
  86  */
  87 int32_t us42GetDistance(rangefinderDev_t *rangefinder)
  88 {
  89     UNUSED(rangefinder);
  90     return us42MeasurementCm;
  91 }
  92
  93 static bool deviceDetect(busDevice_t * busDev)
  94 {
  95     for (int retry = 0; retry < 5; retry++) {
  96         uint8_t inquiryResult;
  97
  98         delay(150);
  99
 100         bool ack = busRead(busDev, US42_I2C_REGISTRY_BASE, &inquiryResult);
 101         if (ack && inquiryResult == US42_I2C_REGISTRY_STATUS_OK) {
 102             return true;
 103         }
 104     };
 105
 106     return false;
 107 }
 108
 109 bool us42Detect(rangefinderDev_t *rangefinder)
 110 {
 111     rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_US42, 0, OWNER_RANGEFINDER);
 112     if (rangefinder->busDev == NULL) {
 113         return false;
 114     }
 115
 116     if (!deviceDetect(rangefinder->busDev)) {
 117         busDeviceDeInit(rangefinder->busDev);
 118         return false;
 119     }
 120
 121     rangefinder->delayMs = RANGEFINDER_US42_TASK_PERIOD_MS;
 122     rangefinder->maxRangeCm = US42_MAX_RANGE_CM;
 123     rangefinder->detectionConeDeciDegrees = US42_DETECTION_CONE_DECIDEGREES;
 124     rangefinder->detectionConeExtendedDeciDegrees = US42_DETECTION_CONE_EXTENDED_DECIDEGREES;
 125
 126     rangefinder->init = &us42Init;
 127     rangefinder->update = &us42Update;
 128     rangefinder->read = &us42GetDistance;
 129
 130     return true;
 131 }
 132 #endif