src/main/drivers/rangefinder/rangefinder_tof10120_i2c.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TOF10120_I2C)
  25
  26 #include "drivers/time.h"
  27 #include "drivers/rangefinder/rangefinder.h"
  28 #include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
  29
  30 #define TOF10120_I2C_MAX_RANGE_CM 200
  31 #define TOF10120_I2C_MAX_RANGE_MM TOF10120_I2C_MAX_RANGE_CM * 10
  32 #define TOF10120_I2C_DETECTION_CONE_DECIDEGREES 300
  33 #define TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES 450
  34
  35 #define TOF10120_I2C_ADDRESS 0x52
  36
  37 #define TOF10120_I2C_REGISTRY_DISTANCE 0x04    // address of first byte for reading filtered distance
  38 #define TOF10120_I2C_REGISTRY_RT_DISTANCE 0x00 // address of first byte for reading real-time raw distance
  39
  40 #define TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG 0x0f
  41 #define TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG 0x09
  42
  43 volatile int32_t tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
  44 static bool isTof10120Responding = false;
  45
  46 static void tof10120i2cInit(rangefinderDev_t *rangefinder)
  47 {
  48     busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01);
  49     delay(100);
  50 }
  51
  52 void tof10120i2cUpdate(rangefinderDev_t *rangefinder)
  53 {
  54     uint8_t buffer[2];
  55     uint16_t distance_mm;
  56
  57     isTof10120Responding = busReadBuf(rangefinder->busDev, TOF10120_I2C_REGISTRY_RT_DISTANCE, buffer, sizeof(buffer));
  58
  59     if (!isTof10120Responding) {
  60         return;
  61     }
  62
  63     distance_mm = (buffer[0] << 8) | buffer[1];
  64
  65     if (distance_mm >= TOF10120_I2C_MAX_RANGE_MM) {
  66         tof10120MeasurementCm = RANGEFINDER_OUT_OF_RANGE;
  67         return;
  68     }
  69
  70     tof10120MeasurementCm = (int32_t)roundf(distance_mm / 10);
  71 }
  72
  73 /**
  74  * Get the distance that was measured by the last pulse, in centimeters.
  75  */
  76 int32_t tof10120i2cGetDistance(rangefinderDev_t *rangefinder)
  77 {
  78     UNUSED(rangefinder);
  79
  80     if (isTof10120Responding) {
  81         return tof10120MeasurementCm;
  82     } else {
  83         return RANGEFINDER_HARDWARE_FAILURE;
  84     }
  85 }
  86
  87 static bool deviceDetect(rangefinderDev_t *rangefinder)
  88 {
  89     busWrite(rangefinder->busDev, TOF10120_I2C_REGISTRY_SENDING_METHOD_CONFIG, 0x01);
  90     delay(100);
  91
  92     uint8_t response = 0;
  93
  94     if (!busRead(rangefinder->busDev, TOF10120_I2C_REGISTRY_BUS_ADDR_CONFIG, &response) || !response) {
  95         return false;
  96     }
  97
  98     return true;
  99 }
 100
 101 bool tof10120Detect(rangefinderDev_t *rangefinder)
 102 {
 103     rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TOF10120_I2C, 0, OWNER_RANGEFINDER);
 104     if (rangefinder->busDev == NULL) {
 105         return false;
 106     }
 107
 108     if (!deviceDetect(rangefinder)) {
 109         busDeviceDeInit(rangefinder->busDev);
 110         return false;
 111     }
 112
 113     rangefinder->delayMs = RANGEFINDER_TOF10120_I2C_TASK_PERIOD_MS;
 114     rangefinder->maxRangeCm = TOF10120_I2C_MAX_RANGE_CM;
 115     rangefinder->detectionConeDeciDegrees = TOF10120_I2C_DETECTION_CONE_DECIDEGREES;
 116     rangefinder->detectionConeExtendedDeciDegrees = TOF10120_I2C_DETECTION_CONE_EXTENDED_DECIDEGREES;
 117
 118     rangefinder->init = &tof10120i2cInit;
 119     rangefinder->update = &tof10120i2cUpdate;
 120     rangefinder->read = &tof10120i2cGetDistance;
 121
 122     return true;
 123 }
 124 #endif