rangefinderTF refactor + add TF-Nova (#14379)

* Add rangefinder TF-Nova

* fix

* Return comments. Removed spaces

* Removed invalid command. Expanded range

* tfmini and tf02 united command for 100hz sampling

* add aray and enum

* add array and enum

* RANGEFINDER - reafactor lidartf

- use info array
- rename some functions
- init as for lidarMT
- config lidar on initialization (functional change)

* RANGEFINDER - common TF detection using rangefinderTYpe

* prevent too frequent lidar reconf

suggested by @coderabbitai

* trace errors in debug

* fix tfFrame length

@coderabbitai

* update public header

@coderabbitai

* Fix problems from upstream merge

* RAngefinder - minor cleanup

---------

Co-authored-by: LarryKarhu <advlazeris@gmail.com>

src/main/drivers/rangefinder/rangefinder_lidartf.c

@@ -34,16 +34,24 @@
 #include "drivers/rangefinder/rangefinder.h"
 #include "drivers/rangefinder/rangefinder_lidartf.h"
 
-#define TF_DEVTYPE_NONE 0
+typedef struct {
-#define TF_DEVTYPE_MINI 1
+    rangefinderType_e rfType;
-#define TF_DEVTYPE_02   2
+    uint16_t rangeMin;
-#define TF_DEVTYPE_NOVA 3
+    uint16_t rangeMax;
+} lidarTFInfo_t;
 
-static uint8_t tfDevtype = TF_DEVTYPE_NONE;
+
+static const lidarTFInfo_t *devInfo = NULL;
+static const lidarTFInfo_t devInfos[] = {
+    { .rfType = RANGEFINDER_TFMINI, .rangeMin = 40, .rangeMax = 1200},
+    { .rfType = RANGEFINDER_TF02,   .rangeMin = 40, .rangeMax = 2200},
+    { .rfType = RANGEFINDER_TFNOVA, .rangeMin = 10, .rangeMax = 1400},
+};
 
 #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)
+#define TF_TASK_PERIOD_MS  10
 
 //
 // Benewake TFmini frame format
@@ -98,21 +106,10 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE;
 // Credibility
 // 1. If Confidence level < 90, unreliable
 // 2. If distance is 14m (1400cm), then OoR.
-//
+
 #define TF_NOVA_FRAME_CONFIDENCE 5
 
-// Maximum ratings
+#define TF_DETECTION_CONE_DECIDEGREES 900  // TODO
-
-#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_NOVA_RANGE_MIN 10
-#define TF_NOVA_RANGE_MAX 1400
-
-#define TF_DETECTION_CONE_DECIDEGREES 900
 
 static serialPort_t *tfSerialPort = NULL;
 
@@ -130,21 +127,35 @@ static uint8_t tfReceivePosition;
 // TFmini and TF02
 // Command for 100Hz sampling (10msec interval)
 // At 100Hz scheduling, skew will cause 10msec delay at the most.
-static const uint8_t tfCmd[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
+// This command format does not match latest Benewake documentation
+static const uint8_t tfConfigCmd[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
 
-static int32_t lidarTFValue;
+static int lidarTFValue;
-static uint16_t lidarTFerrors = 0;
+static unsigned lidarTFerrors = 0;
 
-static void lidarTFSendCommand(void)
+
+static const lidarTFInfo_t* findInfo(rangefinderType_e rfType)
 {
-    switch (tfDevtype) {
+    for (const lidarTFInfo_t* p = devInfos; p < ARRAYEND(devInfos); p++) {
-    case TF_DEVTYPE_MINI:
+        if (p->rfType == rfType) {
-    case TF_DEVTYPE_02:
+            return p;
-        serialWriteBuf(tfSerialPort, tfCmd, sizeof(tfCmd));
+        }
+    }
+    return NULL;
+}
+
+// configure/reconfigure device
+// may be called multiple times (when frames are missing)
+static void lidarTFConfig(rangefinderDev_t *dev, const lidarTFInfo_t* inf)
+{
+    UNUSED(dev);
+    switch (inf->rfType) {
+    case RANGEFINDER_TFMINI:
+    case RANGEFINDER_TF02:
+        serialWriteBuf(tfSerialPort, tfConfigCmd, sizeof(tfConfigCmd));
         break;
     default:
         break;
-
     }
 }
 
@@ -156,13 +167,54 @@ static void lidarTFInit(rangefinderDev_t *dev)
     tfReceivePosition = 0;
 }
 
+static int tfProcessFrame(const uint8_t* frame, int len)
+{
+    UNUSED(len);
+    uint16_t distance = frame[0] | (frame[1] << 8);
+    uint16_t strength = frame[2] | (frame[3] << 8);
+
+    DEBUG_SET(DEBUG_LIDAR_TF, 0, distance);  // 0,1
+    DEBUG_SET(DEBUG_LIDAR_TF, 1, strength);  // 2,3
+    DEBUG_SET(DEBUG_LIDAR_TF, 2, frame[4]);
+    DEBUG_SET(DEBUG_LIDAR_TF, 3, frame[5]);
+
+    // common distance check
+    if (distance < devInfo->rangeMin || distance > devInfo->rangeMax) {
+        return RANGEFINDER_OUT_OF_RANGE;
+    }
+
+    switch (devInfo->rfType) {
+    case RANGEFINDER_TFMINI:
+        if (frame[TF_MINI_FRAME_INTEGRAL_TIME] == 7) {
+            // When integral time is long (7), measured distance tends to be longer by 12~13.
+            distance -= 13;
+        }
+        break;
+    case RANGEFINDER_TF02:
+        if (frame[TF_02_FRAME_SIG] < 7) {
+            return RANGEFINDER_OUT_OF_RANGE;
+
+        }
+        break;
+    case RANGEFINDER_TFNOVA:
+        if (frame[TF_NOVA_FRAME_CONFIDENCE] < 90) {
+            return RANGEFINDER_OUT_OF_RANGE;
+        }
+        break;
+    default:
+        return RANGEFINDER_HARDWARE_FAILURE;  // internal error
+    }
+    // distance is valid
+    return distance;
+}
+
 static void lidarTFUpdate(rangefinderDev_t *dev)
 {
-    UNUSED(dev);
     static timeMs_t lastFrameReceivedMs = 0;
+    static timeMs_t lastReconfMs = 0;
     const timeMs_t timeNowMs = millis();
 
-    if (tfSerialPort == NULL) {
+    if (tfSerialPort == NULL || devInfo == NULL) {
         return;
     }
 
@@ -190,76 +242,37 @@ static void lidarTFUpdate(rangefinderDev_t *dev)
             }
             break;
 
-        case TF_FRAME_STATE_WAIT_CKSUM:
+        case TF_FRAME_STATE_WAIT_CKSUM: {
-            {
+            uint8_t cksum = TF_FRAME_SYNC_BYTE + TF_FRAME_SYNC_BYTE;  // SYNC bytes are checksummed too, but not stored
-                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) {
-
+                lidarTFValue = tfProcessFrame(tfFrame, TF_FRAME_LENGTH);
-                    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;
-
-                    case TF_DEVTYPE_NOVA:
-                        if (distance >= TF_NOVA_RANGE_MIN && distance <= TF_NOVA_RANGE_MAX && tfFrame[TF_NOVA_FRAME_CONFIDENCE] >= 90) {
-                            lidarTFValue = distance;
-                        } else {
-                            lidarTFValue = -1;
-                        }
-                        break;
-                    }
                 lastFrameReceivedMs = timeNowMs;
             } else {
-                    // Checksum error. Simply discard the current frame.
+                // Checksum error. Simply ignore the current frame.
                 ++lidarTFerrors;
-                    //DEBUG_SET(DEBUG_LIDAR_TF, 3, lidarTFerrors);
+                DEBUG_SET(DEBUG_LIDAR_TF, 4, 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 valid frame hasn't been received for more than a timeout, try reinit.
-
+    if (cmp32(timeNowMs, lastFrameReceivedMs) > TF_TIMEOUT_MS
-    if (timeNowMs - lastFrameReceivedMs > TF_TIMEOUT_MS) {
+        && cmp32(timeNowMs, lastReconfMs) > 500) {
-        lidarTFSendCommand();
+        lidarTFConfig(dev, devInfo);
+        lastReconfMs = timeNowMs;    // delay sensor reconf
     }
 }
 
 // Return most recent device output in cm
-
+// TODO - handle timeout; return value only once (see lidarMT)
 static int32_t lidarTFGetDistance(rangefinderDev_t *dev)
 {
     UNUSED(dev);
@@ -267,37 +280,25 @@ static int32_t lidarTFGetDistance(rangefinderDev_t *dev)
     return lidarTFValue;
 }
 
-static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
+bool lidarTFDetect(rangefinderDev_t *dev, rangefinderType_e rfType)
 {
-    const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LIDAR_TF);
+    const lidarTFInfo_t* inf = findInfo(rfType);
+    if (!inf) {
+        return false; // supplied rfType is not TF
+    }
 
+    const 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 = TF_TASK_PERIOD_MS;
-
+    dev->maxRangeCm = inf->rangeMax;
-    dev->delayMs = 10;
-    switch (devtype) {
-    case TF_DEVTYPE_MINI:
-        dev->maxRangeCm = TF_MINI_RANGE_MAX;
-        break;
-    case TF_DEVTYPE_02:
-        dev->maxRangeCm = TF_02_RANGE_MAX;
-        break;
-    case TF_DEVTYPE_NOVA:
-        dev->maxRangeCm = TF_NOVA_RANGE_MAX;
-        break;
-    default:
-        dev->maxRangeCm = 0;
-        break;
-    }
 
     dev->detectionConeDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
     dev->detectionConeExtendedDeciDegrees = TF_DETECTION_CONE_DECIDEGREES;
@@ -306,22 +307,12 @@ static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
     dev->update = &lidarTFUpdate;
     dev->read = &lidarTFGetDistance;
 
+    devInfo = inf;
+
+    // configure device
+    lidarTFConfig(dev, devInfo);
+
     return true;
 }
 
-bool lidarTFminiDetect(rangefinderDev_t *dev)
-{
-    return lidarTFDetect(dev, TF_DEVTYPE_MINI);
-}
-
-bool lidarTF02Detect(rangefinderDev_t *dev)
-{
-    return lidarTFDetect(dev, TF_DEVTYPE_02);
-}
-
-bool lidarTFNovaDetect(rangefinderDev_t *dev)
-{
-    return lidarTFDetect(dev, TF_DEVTYPE_NOVA);
-}
-
 #endif

src/main/drivers/rangefinder/rangefinder_lidartf.h

@@ -20,10 +20,7 @@
 
 #pragma once
 
-#include "common/time.h"
+#include "drivers/rangefinder/rangefinder.h"
+#include "sensors/rangefinder.h"
 
-#define RANGEFINDER_TF_TASK_PERIOD_MS 10
+bool lidarTFDetect(rangefinderDev_t *dev, rangefinderType_e rfType);
-
-bool lidarTFminiDetect(rangefinderDev_t *dev);
-bool lidarTF02Detect(rangefinderDev_t *dev);
-bool lidarTFNovaDetect(rangefinderDev_t *dev);

src/main/sensors/rangefinder.c

@@ -109,29 +109,13 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
             break;
 
+#if defined(USE_RANGEFINDER_TF)
         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_TFNOVA:
-#if defined(USE_RANGEFINDER_TF)
+            if (lidarTFDetect(dev, rangefinderHardwareToUse)) {
-            if (lidarTFNovaDetect(dev)) {
+                rangefinderHardware = rangefinderHardwareToUse;
-                rangefinderHardware = RANGEFINDER_TFNOVA;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(dev->delayMs));
-                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
             }
 #endif
             break;