src/main/drivers/rangefinder/rangefinder_teraranger_evo.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 #include "common/maths.h"
  24
  25 #if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
  26
  27 #include "build/build_config.h"
  28
  29 #include "common/crc.h"
  30
  31 #include "drivers/time.h"
  32 #include "drivers/bus_i2c.h"
  33
  34 #include "drivers/rangefinder/rangefinder.h"
  35 #include "drivers/rangefinder/rangefinder_teraranger_evo.h"
  36
  37 #include "build/debug.h"
  38
  39 #define TERARANGER_EVO_DETECTION_CONE_DECIDEGREES             900
  40 #define TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES    900
  41
  42 #define TERARANGER_EVO_I2C_ADDRESS                    0x31
  43 #define TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING   0x00 // Write this command to the TeraRanger Evo and after a wait of approximately 500us read the 3 byte distance response
  44 #define TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I          0x01 // Write this value to TeraRanger Evo via I2C and the device responds with 0xA
  45 #define TERARANGER_EVO_I2C_REGISTRY_CHANGE_BASE_ADDR  0xA2 // This command assigns a base address that will be memorised by the TerRanger Evo ie. power cycling the Evo will not restore the default I2C address.
  46
  47 #define TERARANGER_EVO_I2C_ANSWER_WHO_AM_I            0xA1
  48
  49 #define TERARANGER_EVO_VALUE_TOO_CLOSE                0x0000
  50 #define TERARANGER_EVO_VALUE_OUT_OF_RANGE             0xffff
  51
  52 static      int16_t     minimumReadingIntervalMs = 50;
  53             uint8_t     triggerValue[0];
  54 volatile    int32_t     teraRangerMeasurementCm;
  55 static      uint32_t    timeOfLastMeasurementMs;
  56 static      uint8_t     dataBuff[3];
  57
  58 static void teraRangerInit(rangefinderDev_t *rangefinder){
  59     busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, triggerValue, 1);
  60 }
  61
  62 void teraRangerUpdate(rangefinderDev_t *rangefinder){
  63     if (busReadBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, dataBuff, 3)) {
  64         teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(((int32_t)dataBuff[0] << 8 | (int32_t)dataBuff[1]));
  65
  66         if(dataBuff[2] == crc8_update(0, &dataBuff, 2)){
  67             if (teraRangerMeasurementCm == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementCm == TERARANGER_EVO_VALUE_OUT_OF_RANGE) {
  68                 teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
  69             }
  70         }
  71     } else {
  72         teraRangerMeasurementCm = 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, TERARANGER_EVO_I2C_ADDRESS, triggerValue, 1);
  81     }
  82 }
  83
  84
  85 int32_t teraRangerGetDistance(rangefinderDev_t *rangefinder){
  86     UNUSED(rangefinder);
  87     return teraRangerMeasurementCm;
  88 }
  89
  90 static bool deviceDetect(busDevice_t * busDev){
  91     for (int retry = 0; retry < 5; retry++) {
  92         uint8_t whoIamResult;
  93
  94         delay(150);
  95
  96         bool ack = busRead(busDev, TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I, &whoIamResult);
  97         if (ack && whoIamResult == TERARANGER_EVO_I2C_ANSWER_WHO_AM_I) {
  98             return true;
  99         }
 100     };
 101
 102     return false;
 103 }
 104
 105 bool teraRangerDetect(rangefinderDev_t *rangefinder){
 106     rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TERARANGER_EVO_I2C, 0, OWNER_RANGEFINDER);
 107     if (rangefinder->busDev == NULL) {
 108         return false;
 109     }
 110
 111     if (!deviceDetect(rangefinder->busDev)) {
 112         busDeviceDeInit(rangefinder->busDev);
 113         return false;
 114     }
 115
 116     rangefinder->delayMs = RANGEFINDER_TERA_EVO_TASK_PERIOD_MS;
 117     rangefinder->maxRangeCm = RANGEFINDER_TERA_EVO_MAX_RANGE_CM;
 118     rangefinder->detectionConeDeciDegrees = TERARANGER_EVO_DETECTION_CONE_DECIDEGREES;
 119     rangefinder->detectionConeExtendedDeciDegrees = TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES;
 120
 121     rangefinder->init = &teraRangerInit;
 122     rangefinder->update = &teraRangerUpdate;
 123     rangefinder->read = &teraRangerGetDistance;
 124
 125     return true;
 126 }
 127
 128 #endif