Merge pull request #9527 from error414/teraranger

terarange evo lidar

docs/Rangefinder.md

@@ -20,6 +20,7 @@ Following rangefinders are supported:
 * UIB - experimental
 * MSP - experimental
 * TOF10120 - small & lightweight laser range sensor, usable up to 200cm
+* TERARANGER EVO - 30cm to 600cm, depends on version https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
 
 ## Connections
 

src/main/CMakeLists.txt

@@ -237,6 +237,8 @@ main_sources(COMMON_SRC
     drivers/rangefinder/rangefinder_us42.h
     drivers/rangefinder/rangefinder_tof10120_i2c.c
     drivers/rangefinder/rangefinder_tof10120_i2c.h
+    drivers/rangefinder/rangefinder_teraranger_evo.c
+    drivers/rangefinder/rangefinder_teraranger_evo.h
 
     drivers/resource.c
     drivers/resource.h

src/main/common/maths.h

@@ -56,6 +56,8 @@
 #define RADIANS_TO_CENTIDEGREES(angle) (((angle) * 100.0f) / RAD)
 #define CENTIDEGREES_TO_RADIANS(angle) (((angle) * 0.01f) * RAD)
 
+#define MILLIMETERS_TO_CENTIMETERS(mm)  (mm / 10.0f)
+
 #define CENTIMETERS_TO_CENTIFEET(cm)    (cm / 0.3048f)
 #define CENTIMETERS_TO_FEET(cm)         (cm / 30.48f)
 #define CENTIMETERS_TO_METERS(cm)       (cm * 0.01f)

src/main/drivers/bus.h

@@ -135,6 +135,7 @@ typedef enum {
     DEVHW_VL53L1X,
     DEVHW_US42,
     DEVHW_TOF10120_I2C,
+    DEVHW_TERARANGER_EVO_I2C,
 
     /* Other hardware */
     DEVHW_MS4525,       // Pitot meter

src/main/drivers/rangefinder/rangefinder_teraranger_evo.c

@@ -0,0 +1,128 @@
+/*
+ * This file is part of Cleanflight.
+ *
+ * Cleanflight is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * Cleanflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "platform.h"
+
+#include "common/maths.h"
+
+#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
+
+#include "build/build_config.h"
+
+#include "common/crc.h"
+
+#include "drivers/time.h"
+#include "drivers/bus_i2c.h"
+
+#include "drivers/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
+
+#include "build/debug.h"
+
+#define TERARANGER_EVO_DETECTION_CONE_DECIDEGREES             900
+#define TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES    900
+
+#define TERARANGER_EVO_I2C_ADDRESS                    0x31
+#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
+#define TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I          0x01 // Write this value to TeraRanger Evo via I2C and the device responds with 0xA
+#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.
+
+#define TERARANGER_EVO_I2C_ANSWER_WHO_AM_I            0xA1
+
+#define TERARANGER_EVO_VALUE_TOO_CLOSE                0x0000
+#define TERARANGER_EVO_VALUE_OUT_OF_RANGE             0xffff
+
+static      int16_t     minimumReadingIntervalMs = 50;
+            uint8_t     triggerValue[0];
+volatile    int32_t     teraRangerMeasurementCm;
+static      uint32_t    timeOfLastMeasurementMs;
+static      uint8_t     dataBuff[3];
+
+static void teraRangerInit(rangefinderDev_t *rangefinder){
+    busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, triggerValue, 1);
+}
+
+void teraRangerUpdate(rangefinderDev_t *rangefinder){
+    if (busReadBuf(rangefinder->busDev, TERARANGER_EVO_I2C_REGISTRY_TRIGGER_READING, dataBuff, 3)) {
+        teraRangerMeasurementCm = MILLIMETERS_TO_CENTIMETERS(((int32_t)dataBuff[0] << 8 | (int32_t)dataBuff[1]));
+
+        if(dataBuff[2] == crc8_update(0, &dataBuff, 2)){
+            if (teraRangerMeasurementCm == TERARANGER_EVO_VALUE_TOO_CLOSE || teraRangerMeasurementCm == TERARANGER_EVO_VALUE_OUT_OF_RANGE) {
+                teraRangerMeasurementCm = RANGEFINDER_OUT_OF_RANGE;
+            }
+        }
+    } else {
+        teraRangerMeasurementCm = RANGEFINDER_HARDWARE_FAILURE;
+    }
+
+    const timeMs_t timeNowMs = millis();
+    if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) {
+        // measurement repeat interval should be greater than minimumReadingIntervalMs
+        // to avoid interference between connective measurements.
+        timeOfLastMeasurementMs = timeNowMs;
+        busWriteBuf(rangefinder->busDev, TERARANGER_EVO_I2C_ADDRESS, triggerValue, 1);
+    }
+}
+
+
+int32_t teraRangerGetDistance(rangefinderDev_t *rangefinder){
+    UNUSED(rangefinder);
+    return teraRangerMeasurementCm;
+}
+
+static bool deviceDetect(busDevice_t * busDev){
+    for (int retry = 0; retry < 5; retry++) {
+        uint8_t whoIamResult;
+
+        delay(150);
+
+        bool ack = busRead(busDev, TERARANGER_EVO_I2C_REGISTRY_WHO_AM_I, &whoIamResult);
+        if (ack && whoIamResult == TERARANGER_EVO_I2C_ANSWER_WHO_AM_I) {
+            return true;
+        }
+    };
+
+    return false;
+}
+
+bool teraRangerDetect(rangefinderDev_t *rangefinder){
+    rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_TERARANGER_EVO_I2C, 0, OWNER_RANGEFINDER);
+    if (rangefinder->busDev == NULL) {
+        return false;
+    }
+
+    if (!deviceDetect(rangefinder->busDev)) {
+        busDeviceDeInit(rangefinder->busDev);
+        return false;
+    }
+
+    rangefinder->delayMs = RANGEFINDER_TERA_EVO_TASK_PERIOD_MS;
+    rangefinder->maxRangeCm = RANGEFINDER_TERA_EVO_MAX_RANGE_CM;
+    rangefinder->detectionConeDeciDegrees = TERARANGER_EVO_DETECTION_CONE_DECIDEGREES;
+    rangefinder->detectionConeExtendedDeciDegrees = TERARANGER_EVO_DETECTION_CONE_EXTENDED_DECIDEGREES;
+
+    rangefinder->init = &teraRangerInit;
+    rangefinder->update = &teraRangerUpdate;
+    rangefinder->read = &teraRangerGetDistance;
+
+    return true;
+}
+
+#endif

src/main/drivers/rangefinder/rangefinder_teraranger_evo.h

@@ -0,0 +1,29 @@
+/*
+ * This file is part of INAV.
+ *
+ * This Source Code Form is subject to the terms of the Mozilla Public
+ * License, v. 2.0. If a copy of the MPL was not distributed with this file,
+ * You can obtain one at http://mozilla.org/MPL/2.0/.
+ *
+ * Alternatively, the contents of this file may be used under the terms
+ * of the GNU General Public License Version 3, as described below:
+ *
+ * This file is free software: you may copy, redistribute and/or modify
+ * it under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or (at your
+ * option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
+ * Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see http://www.gnu.org/licenses/.
+ */
+#pragma once
+
+#define RANGEFINDER_TERA_EVO_TASK_PERIOD_MS 70
+#define RANGEFINDER_TERA_EVO_MAX_RANGE_CM 600 // max distance depends on model https://www.terabee.com/sensors-modules/lidar-tof-range-finders/#individual-distance-measurement-sensors
+
+bool teraRangerDetect(rangefinderDev_t *dev);

src/main/fc/settings.yaml

@@ -7,7 +7,7 @@ tables:
     values: ["NONE", "AUTO", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "ICM42605", "BMI270","LSM6DXX",  "FAKE"]
     enum: accelerationSensor_e
   - name: rangefinder_hardware
-    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE"]
+    values: ["NONE", "SRF10", "VL53L0X", "MSP", "BENEWAKE", "VL53L1X", "US42", "TOF10120_I2C", "FAKE", "TERARANGER_EVO"]
     enum: rangefinderType_e
   - name: mag_hardware
     values: ["NONE", "AUTO", "HMC5883", "AK8975", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", "VCM5883", "MLX90393", "FAKE"]

src/main/sensors/rangefinder.c

@@ -41,6 +41,7 @@
 #include "drivers/rangefinder/rangefinder_vl53l1x.h"
 #include "drivers/rangefinder/rangefinder_virtual.h"
 #include "drivers/rangefinder/rangefinder_us42.h"
+#include "drivers/rangefinder/rangefinder_teraranger_evo.h"
 #include "drivers/rangefinder/rangefinder_tof10120_i2c.h"
 
 #include "fc/config.h"
@@ -88,6 +89,14 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
             break;
 
+            case RANGEFINDER_TERARANGER_EVO:
+#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
+            if (teraRangerDetect(dev)) {
+                rangefinderHardware = RANGEFINDER_TERARANGER_EVO;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TERA_EVO_TASK_PERIOD_MS));
+            }
+#endif
+            break;
             case RANGEFINDER_VL53L0X:
 #if defined(USE_RANGEFINDER_VL53L0X)
             if (vl53l0xDetect(dev)) {

src/main/sensors/rangefinder.h

@@ -22,15 +22,16 @@
 #include "drivers/rangefinder/rangefinder.h"
 
 typedef enum {
-    RANGEFINDER_NONE        = 0,
-    RANGEFINDER_SRF10       = 1,
-    RANGEFINDER_VL53L0X     = 2,
-    RANGEFINDER_MSP         = 3,
-    RANGEFINDER_BENEWAKE    = 4,
-    RANGEFINDER_VL53L1X     = 5,
-    RANGEFINDER_US42        = 6,
-    RANGEFINDER_TOF10102I2C = 7,
-    RANGEFINDER_FAKE        = 8,
+    RANGEFINDER_NONE                = 0,
+    RANGEFINDER_SRF10               = 1,
+    RANGEFINDER_VL53L0X             = 2,
+    RANGEFINDER_MSP                 = 3,
+    RANGEFINDER_BENEWAKE            = 4,
+    RANGEFINDER_VL53L1X             = 5,
+    RANGEFINDER_US42                = 6,
+    RANGEFINDER_TOF10102I2C         = 7,
+    RANGEFINDER_FAKE                = 8,
+    RANGEFINDER_TERARANGER_EVO      = 9,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {

src/main/target/common.h

@@ -83,6 +83,7 @@
 #define USE_RANGEFINDER_VL53L1X
 #define USE_RANGEFINDER_US42
 #define USE_RANGEFINDER_TOF10120_I2C
+#define USE_RANGEFINDER_TERARANGER_EVO_I2C
 
 // Allow default optic flow boards
 #define USE_OPFLOW

src/main/target/common_hardware.c

@@ -341,6 +341,15 @@
     #endif
 #endif
 
+#if defined(USE_RANGEFINDER_TERARANGER_EVO_I2C)
+    #if !defined(TERARANGER_EVO_I2C_BUS) && defined(RANGEFINDER_I2C_BUS)
+        #define TERARANGER_EVO_I2C_BUS RANGEFINDER_I2C_BUS
+    #endif
+    #if defined(TERARANGER_EVO_I2C_BUS)
+    BUSDEV_REGISTER_I2C(busdev_teraranger_evo,       DEVHW_TERARANGER_EVO_I2C,       TERARANGER_EVO_I2C_BUS,       0x31,       NONE,       DEVFLAGS_NONE,  0);
+    #endif
+#endif
+
 #if defined(USE_RANGEFINDER_TOF10120_I2C) && (defined(TOF10120_I2C_BUS) || defined(RANGEFINDER_I2C_BUS))
     #if !defined(TOF10120_I2C_BUS)
         #define TOF10120_I2C_BUS RANGEFINDER_I2C_BUS