mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
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'
This commit is contained in:
LarryKarhu
GitHub
parent
748217f424
commit
60aaf1ef1e
8 changed files with 104 additions and 14 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -26,3 +26,4 @@
|
|||
|
||||
bool lidarTFminiDetect(rangefinderDev_t *dev);
|
||||
bool lidarTF02Detect(rangefinderDev_t *dev);
|
||||
bool lidarTFNovaDetect(rangefinderDev_t *dev);
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ typedef enum {
|
|||
RANGEFINDER_MTF02 = 5,
|
||||
RANGEFINDER_MTF01P = 6,
|
||||
RANGEFINDER_MTF02P = 7,
|
||||
RANGEFINDER_TFNOVA = 8,
|
||||
} rangefinderType_e;
|
||||
|
||||
typedef struct rangefinderConfig_s {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue