diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b0ad031be9..e1c33fd4b0 100644 --- a/src/main/cli/settings.c +++ b/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 diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 1756ba3d80..296e176904 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/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} }; diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.c b/src/main/drivers/rangefinder/rangefinder_lidartf.c index 07d0c330ed..643b66694a 100644 --- a/src/main/drivers/rangefinder/rangefinder_lidartf.c +++ b/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 }; - -// TF02 -// Same as TFmini for now.. -static uint8_t tfCmdTF02[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 }; +static const uint8_t tfCmd[] = { 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; + } } @@ -173,7 +193,7 @@ static void lidarTFUpdate(rangefinderDev_t *dev) 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++) { + for (int i = 0; i < TF_FRAME_LENGTH; i++) { cksum += tfFrame[i]; } @@ -192,7 +212,7 @@ static void lidarTFUpdate(rangefinderDev_t *dev) 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. + // When integral time is long (7), measured distance tends to be longer by 12~13. lidarTFValue -= 13; } } else { @@ -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 diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.h b/src/main/drivers/rangefinder/rangefinder_lidartf.h index 5ac34d4c62..cd786a79bc 100644 --- a/src/main/drivers/rangefinder/rangefinder_lidartf.h +++ b/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); diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index eabb04d543..4cdb0f4d56 100644 --- a/src/main/osd/osd.h +++ b/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; diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index c4f92181b4..da59d7fae1 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -50,6 +50,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: ------------------------- @@ -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) diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 62790f036d..e243b5027f 100644 --- a/src/main/sensors/rangefinder.c +++ b/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: diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 6cc9684af1..7d4ba30e53 100644 --- a/src/main/sensors/rangefinder.h +++ b/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 {