diff --git a/make/source.mk b/make/source.mk
index a773314a5b..788fd023b2 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -145,6 +145,7 @@ FC_SRC = \
drivers/display_ug2864hsweg01.c \
drivers/light_ws2811strip.c \
drivers/rangefinder/rangefinder_hcsr04.c \
+ drivers/rangefinder/rangefinder_lidartf.c \
drivers/serial_escserial.c \
drivers/vtx_common.c \
flight/navigation.c \
diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index 0c1df15c7a..6f3a94a1b0 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -58,4 +58,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"FPORT",
"RANGEFINDER",
"RANGEFINDER_QUALITY",
+ "LIDAR_TF",
};
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 5640a2b3f7..41960319e0 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -76,6 +76,7 @@ typedef enum {
DEBUG_FPORT,
DEBUG_RANGEFINDER,
DEBUG_RANGEFINDER_QUALITY,
+ DEBUG_LIDAR_TF,
DEBUG_COUNT
} debugType_e;
diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.c b/src/main/drivers/rangefinder/rangefinder_lidartf.c
new file mode 100644
index 0000000000..367caedba6
--- /dev/null
+++ b/src/main/drivers/rangefinder/rangefinder_lidartf.c
@@ -0,0 +1,276 @@
+/*
+ * 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 .
+ */
+
+#include
+#include
+
+#include "platform.h"
+
+#ifdef USE_RANGEFINDER_TF
+
+#include "build/debug.h"
+#include "build/build_config.h"
+
+#include "io/serial.h"
+
+#include "drivers/time.h"
+#include "drivers/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/rangefinder_lidartf.h"
+
+#define TF_DEVTYPE_NONE 0
+#define TF_DEVTYPE_MINI 1
+#define TF_DEVTYPE_02 2
+
+static uint8_t tfDevtype = TF_DEVTYPE_NONE;
+
+#define TF_FRAME_LENGTH 6 // Excluding sync bytes (0x59) x 2 and checksum
+#define TF_FRAME_SYNC_BYTE 0x59
+#define TF_TIMEOUT_MS (100 * 2)
+
+//
+// Benewake TFmini frame format
+// Byte Off Description
+// 1 - SYNC
+// 2 - SYNC
+// 3 0 Measured distance (LSB)
+// 4 1 Measured distance (MSB)
+// 5 2 Signal strength (LSB)
+// 6 3 Signal strength (MSB)
+// 7 4 Integral time
+// 8 5 Reserved
+// 9 - Checksum (Unsigned 8-bit sum of bytes 0~7)
+//
+// Credibility
+// 1. If distance is 12m (1200cm), then OoR.
+//
+#define TF_MINI_FRAME_INTEGRAL_TIME 4
+
+//
+// Benewake TF02 frame format (From SJ-GU-TF02-01 Version: A01)
+// Byte Off Description
+// 1 - SYNC
+// 2 - SYNC
+// 3 0 Measured distance (LSB)
+// 4 1 Measured distance (MSB)
+// 5 2 Signal strength (LSB)
+// 6 3 Signal strength (MSB)
+// 7 4 SIG (Reliability in 1~8, less than 7 is unreliable)
+// 8 5 TIME (Exposure time, 3 or 6)
+// 9 - Checksum (Unsigned 8-bit sum of bytes 0~7)
+//
+// Credibility
+// 1. If SIG is less than 7, unreliable
+// 2. If distance is 22m (2200cm), then OoR.
+//
+#define TF_02_FRAME_SIG 4
+
+// Maximum ratings
+
+#define TF_MINI_RANGE_MIN 40
+#define TF_MINI_RANGE_MAX 1200
+
+#define TF_02_RANGE_MIN 40
+#define TF_02_RANGE_MAX 2200
+
+#define TF_DETECTION_CONE_DECIDEGREES 900
+
+static serialPort_t *tfSerialPort = NULL;
+
+typedef enum {
+ TF_FRAME_STATE_WAIT_START1,
+ TF_FRAME_STATE_WAIT_START2,
+ TF_FRAME_STATE_READING_PAYLOAD,
+ TF_FRAME_STATE_WAIT_CKSUM,
+} tfFrameState_e;
+
+static tfFrameState_e tfFrameState;
+static uint8_t tfFrame[TF_FRAME_LENGTH];
+static uint8_t tfReceivePosition;
+
+// TFmini
+// Command for 100Hz sampling (10msec interval)
+// At 100Hz scheduling, skew will cause 10msec delay at the most.
+static uint8_t tfCmdTFmini[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
+
+// TF02
+// Same as TFmini for now..
+static uint8_t tfCmdTF02[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
+
+static int32_t lidarTFValue;
+static uint16_t lidarTFerrors = 0;
+
+static void lidarTFSendCommand(void)
+{
+ switch (tfDevtype) {
+ case TF_DEVTYPE_MINI:
+ serialWriteBuf(tfSerialPort, tfCmdTFmini, sizeof(tfCmdTFmini));
+ break;
+ case TF_DEVTYPE_02:
+ serialWriteBuf(tfSerialPort, tfCmdTF02, sizeof(tfCmdTF02));
+ break;
+ }
+}
+
+void lidarTFInit(rangefinderDev_t *dev)
+{
+ UNUSED(dev);
+
+ tfFrameState = TF_FRAME_STATE_WAIT_START1;
+ tfReceivePosition = 0;
+}
+
+void lidarTFUpdate(rangefinderDev_t *dev)
+{
+ UNUSED(dev);
+ static timeMs_t lastFrameReceivedMs = 0;
+ const timeMs_t timeNowMs = millis();
+
+ if (tfSerialPort == NULL) {
+ return;
+ }
+
+ while (serialRxBytesWaiting(tfSerialPort)) {
+ uint8_t c = serialRead(tfSerialPort);
+ switch (tfFrameState) {
+ case TF_FRAME_STATE_WAIT_START1:
+ if (c == TF_FRAME_SYNC_BYTE) {
+ tfFrameState = TF_FRAME_STATE_WAIT_START2;
+ }
+ break;
+
+ case TF_FRAME_STATE_WAIT_START2:
+ if (c == TF_FRAME_SYNC_BYTE) {
+ tfFrameState = TF_FRAME_STATE_READING_PAYLOAD;
+ } else {
+ tfFrameState = TF_FRAME_STATE_WAIT_START1;
+ }
+ break;
+
+ case TF_FRAME_STATE_READING_PAYLOAD:
+ tfFrame[tfReceivePosition++] = c;
+ if (tfReceivePosition == TF_FRAME_LENGTH) {
+ tfFrameState = TF_FRAME_STATE_WAIT_CKSUM;
+ }
+ break;
+
+ case TF_FRAME_STATE_WAIT_CKSUM:
+ {
+ uint8_t cksum = TF_FRAME_SYNC_BYTE + TF_FRAME_SYNC_BYTE;
+ for (int i = 0 ; i < TF_FRAME_LENGTH ; i++) {
+ cksum += tfFrame[i];
+ }
+
+ if (c == cksum) {
+
+ uint16_t distance = tfFrame[0] | (tfFrame[1] << 8);
+ uint16_t strength = tfFrame[2] | (tfFrame[3] << 8);
+
+ DEBUG_SET(DEBUG_LIDAR_TF, 0, distance);
+ DEBUG_SET(DEBUG_LIDAR_TF, 1, strength);
+ DEBUG_SET(DEBUG_LIDAR_TF, 2, tfFrame[4]);
+ DEBUG_SET(DEBUG_LIDAR_TF, 3, tfFrame[5]);
+
+ switch (tfDevtype) {
+ case TF_DEVTYPE_MINI:
+ if (distance >= TF_MINI_RANGE_MIN && distance < TF_MINI_RANGE_MAX) {
+ lidarTFValue = distance;
+ if (tfFrame[TF_MINI_FRAME_INTEGRAL_TIME] == 7) {
+ // When integral time is long (7), measured distance tends to be longer by 12~13.
+ lidarTFValue -= 13;
+ }
+ } else {
+ lidarTFValue = -1;
+ }
+ break;
+
+ case TF_DEVTYPE_02:
+ if (distance >= TF_02_RANGE_MIN && distance < TF_02_RANGE_MAX && tfFrame[TF_02_FRAME_SIG] >= 7) {
+ lidarTFValue = distance;
+ } else {
+ lidarTFValue = -1;
+ }
+ break;
+ }
+ lastFrameReceivedMs = timeNowMs;
+ } else {
+ // Checksum error. Simply discard the current frame.
+ ++lidarTFerrors;
+ //DEBUG_SET(DEBUG_LIDAR_TF, 3, lidarTFerrors);
+ }
+ }
+
+ tfFrameState = TF_FRAME_STATE_WAIT_START1;
+ tfReceivePosition = 0;
+
+ break;
+ }
+ }
+
+ // If valid frame hasn't been received for more than a timeout, resend command.
+
+ if (timeNowMs - lastFrameReceivedMs > TF_TIMEOUT_MS) {
+ lidarTFSendCommand();
+ }
+}
+
+// Return most recent device output in cm
+
+int32_t lidarTFGetDistance(rangefinderDev_t *dev)
+{
+ UNUSED(dev);
+
+ return lidarTFValue;
+}
+
+static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
+{
+ serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LIDAR_TF);
+
+ if (!portConfig) {
+ return false;
+ }
+
+ tfSerialPort = openSerialPort(portConfig->identifier, FUNCTION_LIDAR_TF, NULL, NULL, 115200, MODE_RXTX, 0);
+
+ if (tfSerialPort == NULL) {
+ return false;
+ }
+
+ tfDevtype = devtype;
+
+ dev->delayMs = 10;
+ dev->maxRangeCm = (devtype == TF_DEVTYPE_MINI) ? TF_MINI_RANGE_MAX : TF_02_RANGE_MAX;
+ dev->detectionConeDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
+ dev->detectionConeExtendedDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
+
+ dev->init = &lidarTFInit;
+ dev->update = &lidarTFUpdate;
+ dev->read = &lidarTFGetDistance;
+
+ return true;
+}
+
+bool lidarTFminiDetect(rangefinderDev_t *dev)
+{
+ return lidarTFDetect(dev, TF_DEVTYPE_MINI);
+}
+
+bool lidarTF02Detect(rangefinderDev_t *dev)
+{
+ return lidarTFDetect(dev, TF_DEVTYPE_02);
+}
+#endif
diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.h b/src/main/drivers/rangefinder/rangefinder_lidartf.h
new file mode 100644
index 0000000000..b6e94617eb
--- /dev/null
+++ b/src/main/drivers/rangefinder/rangefinder_lidartf.h
@@ -0,0 +1,25 @@
+/*
+ * 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 .
+ */
+
+#pragma once
+
+#include "common/time.h"
+
+#define RANGEFINDER_TF_TASK_PERIOD_MS 10
+
+bool lidarTFminiDetect(rangefinderDev_t *dev);
+bool lidarTF02Detect(rangefinderDev_t *dev);
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index ad8fb541a2..b389756254 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -112,7 +112,7 @@ const char * const lookupTableMagHardware[] = {
#endif
#if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
const char * const lookupTableRangefinderHardware[] = {
- "NONE", "HCSR04"
+ "NONE", "HCSR04", "TFMINI", "TF02"
};
#endif
diff --git a/src/main/io/serial.h b/src/main/io/serial.h
index 22377320be..068b91808e 100644
--- a/src/main/io/serial.h
+++ b/src/main/io/serial.h
@@ -45,6 +45,7 @@ typedef enum {
FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096
FUNCTION_VTX_TRAMP = (1 << 13), // 8192
FUNCTION_RCDEVICE = (1 << 14), // 16384
+ FUNCTION_LIDAR_TF = (1 << 15), // 32768
} serialPortFunction_e;
typedef enum {
diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c
index f3c8b32d19..d1de908b71 100644
--- a/src/main/sensors/rangefinder.c
+++ b/src/main/sensors/rangefinder.c
@@ -37,8 +37,9 @@
#include "drivers/io.h"
#include "drivers/time.h"
-#include "drivers/rangefinder/rangefinder_hcsr04.h"
#include "drivers/rangefinder/rangefinder.h"
+#include "drivers/rangefinder/rangefinder_hcsr04.h"
+#include "drivers/rangefinder/rangefinder_lidartf.h"
#include "fc/config.h"
#include "fc/runtime_config.h"
@@ -143,6 +144,24 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
#endif
break;
+ case RANGEFINDER_TFMINI:
+#if defined(USE_RANGEFINDER_TF)
+ if (lidarTFminiDetect(dev)) {
+ rangefinderHardware = RANGEFINDER_TFMINI;
+ rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
+ }
+#endif
+ break;
+
+ case RANGEFINDER_TF02:
+#if defined(USE_RANGEFINDER_TF)
+ if (lidarTF02Detect(dev)) {
+ rangefinderHardware = RANGEFINDER_TF02;
+ rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
+ }
+#endif
+ break;
+
case RANGEFINDER_NONE:
rangefinderHardware = RANGEFINDER_NONE;
break;
diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h
index 183d40a07a..1ef961be8a 100644
--- a/src/main/sensors/rangefinder.h
+++ b/src/main/sensors/rangefinder.h
@@ -24,10 +24,13 @@
typedef enum {
RANGEFINDER_NONE = 0,
RANGEFINDER_HCSR04 = 1,
- RANGEFINDER_SRF10 = 2,
- RANGEFINDER_HCSR04I2C = 3,
- RANGEFINDER_VL53L0X = 4,
- RANGEFINDER_UIB = 5,
+ RANGEFINDER_TFMINI = 2,
+ RANGEFINDER_TF02 = 3,
+ // Following sensors are to be ported from iNav
+ RANGEFINDER_SRF10 = 4,
+ RANGEFINDER_HCSR04I2C = 5,
+ RANGEFINDER_VL53L0X = 6,
+ RANGEFINDER_UIB = 7,
} rangefinderType_e;
typedef struct rangefinderConfig_s {
diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h
index 21c856a5cc..4b99833006 100644
--- a/src/main/target/OMNIBUSF4/target.h
+++ b/src/main/target/OMNIBUSF4/target.h
@@ -247,6 +247,7 @@
#define USE_RANGEFINDER_HCSR04
#define RANGEFINDER_HCSR04_TRIGGER_PIN PA1
#define RANGEFINDER_HCSR04_ECHO_PIN PA8
+#define USE_RANGEFINDER_TF
#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL