src/main/drivers/rangefinder/rangefinder_srf10.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_SRF10)
  24
  25 #include "build/build_config.h"
  26
  27
  28 #include "drivers/time.h"
  29 #include "drivers/bus_i2c.h"
  30
  31 #include "drivers/rangefinder/rangefinder.h"
  32 #include "drivers/rangefinder/rangefinder_srf10.h"
  33
  34 // Technical specification is at: http://robot-electronics.co.uk/htm/srf10tech.htm
  35 #define SRF10_MAX_RANGE_CM 600 // 6m, from SFR10 spec sheet
  36 // see http://www.robot-electronics.co.uk/htm/sonar_faq.htm for cone angles
  37 // FAQ states 55 degrees, conservatively reduced to 50 degrees here
  38 #define SRF10_DETECTION_CONE_DECIDEGREES 500
  39 #define SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES 500
  40
  41 // from spec sheet, when range set to 1100cm
  42 // speed of sound is 340m/s, echo time for 1100cm is 65ms
  43 #define SRF10_MinimumFiringIntervalFor1100cmRangeMs 65
  44 // echo time for 600cm is 36ms, round this up to 40
  45 #define SRF10_MinimumFiringIntervalFor600cmRangeMs 40
  46
  47 // SRF10 hardware constants
  48 #define SRF10_Address 0xE0
  49 #define SRF10_AddressI2C (SRF10_Address>>1) // the I2C 7 bit address
  50
  51 #define SRF10_READ_SoftwareRevision 0x00
  52 #define SRF10_READ_Unused 0x01 // (read value returned is 0x80)
  53 #define SRF10_READ_Unused_ReturnValue 0x80
  54 #define SRF10_READ_RangeHighByte 0x02
  55 #define SRF10_READ_RangeLowByte 0x03
  56
  57 #define SRF10_WRITE_CommandRegister 0x00
  58 #define SRF10_WRITE_MaxGainRegister 0x01 //(default gain value is 16)
  59 #define SRF10_WRITE_RangeRegister 0x02 //(default range value is 255)
  60
  61 #define SRF10_COMMAND_InitiateRangingInches 0x50
  62 #define SRF10_COMMAND_InitiateRangingCm 0x51
  63 #define SRF10_COMMAND_InitiateRangingMicroSeconds 0x53
  64
  65 #define SRF10_COMMAND_SetGain_40 0x00
  66 #define SRF10_COMMAND_SetGain_100 0x06
  67 #define SRF10_COMMAND_SetGain_200 0x09
  68 #define SRF10_COMMAND_SetGain_300 0x0B
  69 #define SRF10_COMMAND_SetGain_400 0x0D
  70 #define SRF10_COMMAND_SetGain_500 0x0E
  71 #define SRF10_COMMAND_SetGain_600 0x0F
  72 #define SRF10_COMMAND_SetGain_700 0x10 // default and maximum
  73 #define SRF10_COMMAND_SetGain_Max 0x10 // default
  74
  75 #define SRF10_COMMAND_ChangeAddress1 0xA0
  76 #define SRF10_COMMAND_ChangeAddress2 0xAA
  77 #define SRF10_COMMAND_ChangeAddress3 0xA5
  78
  79 // The range is (RangeRegister + 1) * 43mm
  80 #define SRF10_RangeValue43mm 0
  81 #define SRF10_RangeValue86mm 1
  82 #define SRF10_RangeValue1m 24
  83 #define SRF10_RangeValue4m 93
  84 #define SRF10_RangeValue6m 139 // maximum range
  85 #define SRF10_RangeValue11m 0xFF // exceeds actual maximum range
  86
  87 STATIC_UNIT_TESTED volatile int32_t srf10measurementCm = RANGEFINDER_OUT_OF_RANGE;
  88 static int16_t minimumFiringIntervalMs;
  89 static uint32_t timeOfLastMeasurementMs;
  90 static bool isSensorResponding = true;
  91
  92 static void srf10_init(rangefinderDev_t * rangefinder)
  93 {
  94     busWrite(rangefinder->busDev, SRF10_WRITE_MaxGainRegister, SRF10_COMMAND_SetGain_Max);
  95     busWrite(rangefinder->busDev, SRF10_WRITE_RangeRegister, SRF10_RangeValue6m);
  96
  97     // initiate first ranging command
  98     busWrite(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm);
  99
 100     timeOfLastMeasurementMs = millis();
 101 }
 102
 103 /*
 104  * Start a range reading
 105  * Called periodically by the scheduler
 106  */
 107 static void srf10_start_reading(rangefinderDev_t * rangefinder)
 108 {
 109     uint8_t revision;
 110
 111     // check if there is a measurement outstanding, 0xFF is returned if no measurement
 112     isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_SoftwareRevision, &revision);
 113
 114     if (isSensorResponding && revision != 0xFF) {
 115         // there is a measurement
 116         uint8_t lowByte, highByte;
 117
 118         isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeLowByte, &lowByte);
 119         isSensorResponding = busRead(rangefinder->busDev, SRF10_READ_RangeHighByte, &highByte);
 120
 121         srf10measurementCm =  highByte << 8 | lowByte;
 122
 123         if (srf10measurementCm > SRF10_MAX_RANGE_CM) {
 124             srf10measurementCm = RANGEFINDER_OUT_OF_RANGE;
 125         }
 126     }
 127
 128     const timeMs_t timeNowMs = millis();
 129     if (timeNowMs > timeOfLastMeasurementMs + minimumFiringIntervalMs) {
 130         // measurement repeat interval should be greater than minimumFiringIntervalMs
 131         // to avoid interference between connective measurements.
 132         timeOfLastMeasurementMs = timeNowMs;
 133         busWrite(rangefinder->busDev, SRF10_WRITE_CommandRegister, SRF10_COMMAND_InitiateRangingCm);
 134     }
 135 }
 136
 137 /**
 138  * Get the distance that was measured by the last pulse, in centimeters.
 139  */
 140 static int32_t srf10_get_distance(rangefinderDev_t *dev)
 141 {
 142     UNUSED(dev);
 143     if (isSensorResponding) {
 144         return srf10measurementCm;
 145     }
 146     else {
 147         return RANGEFINDER_HARDWARE_FAILURE;
 148     }
 149 }
 150
 151 static bool deviceDetect(busDevice_t * busDev)
 152 {
 153     for (int retry = 0; retry < 5; retry++) {
 154         uint8_t inquiryResult;
 155
 156         delay(150);
 157
 158         bool ack = busRead(busDev, SRF10_READ_Unused, &inquiryResult);
 159         if (ack && inquiryResult == SRF10_READ_Unused_ReturnValue) {
 160             return true;
 161         }
 162     };
 163
 164     return false;
 165 }
 166
 167
 168 bool srf10Detect(rangefinderDev_t * rangefinder)
 169 {
 170     rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_SRF10, 0, OWNER_RANGEFINDER);
 171     if (rangefinder->busDev == NULL) {
 172         return false;
 173     }
 174
 175     if (!deviceDetect(rangefinder->busDev)) {
 176         busDeviceDeInit(rangefinder->busDev);
 177         return false;
 178     }
 179
 180     rangefinder->delayMs = SRF10_MinimumFiringIntervalFor600cmRangeMs + 10; // set up the SRF10 hardware for a range of 6m + margin of 10ms
 181     rangefinder->maxRangeCm = SRF10_MAX_RANGE_CM;
 182     rangefinder->detectionConeDeciDegrees = SRF10_DETECTION_CONE_DECIDEGREES;
 183     rangefinder->detectionConeExtendedDeciDegrees = SRF10_DETECTION_CONE_EXTENDED_DECIDEGREES;
 184
 185     rangefinder->init = &srf10_init;
 186     rangefinder->update = &srf10_start_reading;
 187     rangefinder->read = &srf10_get_distance;
 188
 189     return true;
 190 }
 191 #endif