Add rangefinder TF-Nova (#14376)

* 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

* rollback

* add lidar data output to osd

* tabs delete

* returned whitespaces

* switching OSD

* delete icon 'lidar'

src/main/cli/settings.c

@@ -202,7 +202,7 @@ const char * const lookupTableMagHardware[] = {
 #endif
 #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER)
 const char * const lookupTableRangefinderHardware[] = {
-    "NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P"
+    "NONE", "HCSR04", "TFMINI", "TF02", "MTF01", "MTF02", "MTF01P", "MTF02P", "TFNOVA"
 };
 #endif
 #if defined(USE_SENSOR_NAMES) || defined(USE_OPTICALFLOW)
@@ -1671,6 +1671,9 @@ const clivalue_t valueTable[] = {
 #ifdef USE_CRAFTNAME_MSGS
     { "osd_craftname_msgs",   VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_OSD_CONFIG, offsetof(osdConfig_t, osd_craftname_msgs) },
 #endif //USE_CRAFTNAME_MSGS
+#ifdef USE_RANGEFINDER
+    { "osd_lidar_dist_pos",         VAR_UINT16  | MASTER_VALUE, .config.minmaxUnsigned = { 0, OSD_POSCFG_MAX }, PG_OSD_ELEMENT_CONFIG, offsetof(osdElementConfig_t, item_pos[OSD_LIDAR_DIST]) },
+#endif //USE_RANGEFINDER
 #endif // end of #ifdef USE_OSD
 
 // PG_SYSTEM_CONFIG

src/main/cms/cms_menu_osd.c

@@ -175,6 +175,9 @@ const OSD_Entry menuOsdActiveElemsEntries[] =
     {"CAMERA FRAME",       OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_CAMERA_FRAME]},
     {"TOTAL FLIGHTS",      OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_TOTAL_FLIGHTS]},
     {"AUX VALUE",          OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_AUX_VALUE]},
+#ifdef USE_RANGEFINDER
+    {"LIDAR DIST",         OME_VISIBLE | DYNAMIC, NULL, &osdConfig_item_pos[OSD_LIDAR_DIST]},
+#endif
     {"BACK",               OME_Back,    NULL, NULL},
     {NULL,                 OME_END,     NULL, NULL}
 };

src/main/drivers/rangefinder/rangefinder_lidartf.c

@@ -37,6 +37,7 @@
 #define TF_DEVTYPE_NONE 0
 #define TF_DEVTYPE_MINI 1
 #define TF_DEVTYPE_02   2
+#define TF_DEVTYPE_NOVA 3
 
 static uint8_t tfDevtype = TF_DEVTYPE_NONE;
 
@@ -81,6 +82,25 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE;
 //
 #define TF_02_FRAME_SIG 4
 
+//
+// Benewake TFnova 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   Temp (Chip Temperature, degrees Celsius)
+// 8    5   Confidence (Confidence level 0-100)
+// 9    -   Checksum (Unsigned 8-bit sum of bytes 0~7)
+//
+// 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_MINI_RANGE_MIN 40
@@ -89,6 +109,9 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE;
 #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;
@@ -104,14 +127,10 @@ static tfFrameState_e tfFrameState;
 static uint8_t tfFrame[TF_FRAME_LENGTH];
 static uint8_t tfReceivePosition;
 
-// TFmini
+// TFmini and TF02
 // 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 };
+static const uint8_t tfCmd[] = { 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;
@@ -120,11 +139,12 @@ 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));
+        serialWriteBuf(tfSerialPort, tfCmd, sizeof(tfCmd));
         break;
+    default:
+        break;
+
     }
 }
 
@@ -207,6 +227,14 @@ static void lidarTFUpdate(rangefinderDev_t *dev)
                             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 {
@@ -256,7 +284,21 @@ static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype)
     tfDevtype = devtype;
 
     dev->delayMs = 10;
-    dev->maxRangeCm = (devtype == TF_DEVTYPE_MINI) ? TF_MINI_RANGE_MAX : TF_02_RANGE_MAX;
+    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;
 
@@ -276,4 +318,10 @@ 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

@@ -26,3 +26,4 @@
 
 bool lidarTFminiDetect(rangefinderDev_t *dev);
 bool lidarTF02Detect(rangefinderDev_t *dev);
+bool lidarTFNovaDetect(rangefinderDev_t *dev);

src/main/osd/osd.h

@@ -194,6 +194,7 @@ typedef enum {
     OSD_CUSTOM_MSG1,
     OSD_CUSTOM_MSG2,
     OSD_CUSTOM_MSG3,
+    OSD_LIDAR_DIST,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/osd/osd_elements.c

@@ -51,6 +51,8 @@
     Add the mapping for the element ID to the background drawing function to the
     osdElementBackgroundFunction array.
     
+    You should also add a corresponding entry to the file: cms_menu_osd.c
+
     Accelerometer reqirement:
     -------------------------
     If the new element utilizes the accelerometer, add it to the osdElementsNeedAccelerometer() function.
@@ -170,6 +172,7 @@
 #include "sensors/barometer.h"
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
+#include "sensors/rangefinder.h"
 
 #ifdef USE_GPS_PLUS_CODES
 // located in lib/main/google/olc
@@ -320,7 +323,6 @@ int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
     }
 }
 #endif
-
 static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementType_e variantType)
 {
     static const struct {
@@ -682,6 +684,22 @@ char osdGetTemperatureSymbolForSelectedUnit(void)
 // Element drawing functions
 // *************************
 
+#ifdef USE_RANGEFINDER
+static void osdElementLidarDist(osdElementParms_t *element)
+{
+    int16_t dist = rangefinderGetLatestAltitude();
+
+    if (dist > 0) {
+
+        tfp_sprintf(element->buff, "RF:%3d", dist);
+
+    } else {
+
+        tfp_sprintf(element->buff, "RF:---");
+    }
+}
+#endif
+
 #ifdef USE_OSD_ADJUSTMENTS
 static void osdElementAdjustmentRange(osdElementParms_t *element)
 {
@@ -1918,6 +1936,9 @@ static const uint8_t osdElementDisplayOrder[] = {
     OSD_SYS_VTX_TEMP,
     OSD_SYS_FAN_SPEED,
 #endif
+#ifdef USE_RANGEFINDER
+    OSD_LIDAR_DIST,
+#endif
 };
 
 // Define the mapping between the OSD element id and the function to draw it
@@ -2062,6 +2083,9 @@ const osdElementDrawFn osdElementDrawFunction[OSD_ITEM_COUNT] = {
     [OSD_SYS_VTX_TEMP]            = osdElementSys,
     [OSD_SYS_FAN_SPEED]           = osdElementSys,
 #endif
+#ifdef USE_RANGEFINDER
+    [OSD_LIDAR_DIST]              = osdElementLidarDist,
+#endif
 };
 
 // Define the mapping between the OSD element id and the function to draw its background (static part)

src/main/sensors/rangefinder.c

@@ -127,6 +127,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar
 #endif
             break;
 
+        case RANGEFINDER_TFNOVA:
+#if defined(USE_RANGEFINDER_TF)
+            if (lidarTFNovaDetect(dev)) {
+                rangefinderHardware = RANGEFINDER_TFNOVA;
+                rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
+            }
+#endif
+            break;
+
 #if defined(USE_RANGEFINDER_MT)
         case RANGEFINDER_MTF01:
         case RANGEFINDER_MTF02:

src/main/sensors/rangefinder.h

@@ -35,6 +35,7 @@ typedef enum {
     RANGEFINDER_MTF02       = 5,
     RANGEFINDER_MTF01P      = 6,
     RANGEFINDER_MTF02P      = 7,
+    RANGEFINDER_TFNOVA      = 8,
 } rangefinderType_e;
 
 typedef struct rangefinderConfig_s {