mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
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>
This commit is contained in:
Petr Ledvina
GitHub
parent
6b3bdef6fd
commit
31e5644b56
3 changed files with 120 additions and 148 deletions
|
|
@ -34,16 +34,24 @@
|
|||
#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
|
||||
#define TF_DEVTYPE_NOVA 3
|
||||
typedef struct {
|
||||
rangefinderType_e rfType;
|
||||
uint16_t rangeMin;
|
||||
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_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
|
||||
#define TF_DETECTION_CONE_DECIDEGREES 900 // TODO
|
||||
|
||||
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 uint16_t lidarTFerrors = 0;
|
||||
static int lidarTFValue;
|
||||
static unsigned lidarTFerrors = 0;
|
||||
|
||||
static void lidarTFSendCommand(void)
|
||||
|
||||
static const lidarTFInfo_t* findInfo(rangefinderType_e rfType)
|
||||
{
|
||||
switch (tfDevtype) {
|
||||
case TF_DEVTYPE_MINI:
|
||||
case TF_DEVTYPE_02:
|
||||
serialWriteBuf(tfSerialPort, tfCmd, sizeof(tfCmd));
|
||||
for (const lidarTFInfo_t* p = devInfos; p < ARRAYEND(devInfos); p++) {
|
||||
if (p->rfType == rfType) {
|
||||
return p;
|
||||
}
|
||||
}
|
||||
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:
|
||||
{
|
||||
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;
|
||||
|
||||
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.
|
||||
++lidarTFerrors;
|
||||
//DEBUG_SET(DEBUG_LIDAR_TF, 3, lidarTFerrors);
|
||||
}
|
||||
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
|
||||
for (int i = 0; i < TF_FRAME_LENGTH; i++) {
|
||||
cksum += tfFrame[i];
|
||||
}
|
||||
|
||||
if (c == cksum) {
|
||||
lidarTFValue = tfProcessFrame(tfFrame, TF_FRAME_LENGTH);
|
||||
lastFrameReceivedMs = timeNowMs;
|
||||
} else {
|
||||
// Checksum error. Simply ignore the current frame.
|
||||
++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 (timeNowMs - lastFrameReceivedMs > TF_TIMEOUT_MS) {
|
||||
lidarTFSendCommand();
|
||||
// If valid frame hasn't been received for more than a timeout, try reinit.
|
||||
if (cmp32(timeNowMs, lastFrameReceivedMs) > TF_TIMEOUT_MS
|
||||
&& cmp32(timeNowMs, lastReconfMs) > 500) {
|
||||
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 = 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->delayMs = TF_TASK_PERIOD_MS;
|
||||
dev->maxRangeCm = inf->rangeMax;
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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 lidarTFminiDetect(rangefinderDev_t *dev);
|
||||
bool lidarTF02Detect(rangefinderDev_t *dev);
|
||||
bool lidarTFNovaDetect(rangefinderDev_t *dev);
|
||||
bool lidarTFDetect(rangefinderDev_t *dev, rangefinderType_e rfType);
|
||||
|
|
|
|||
|
|
@ -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 (lidarTFNovaDetect(dev)) {
|
||||
rangefinderHardware = RANGEFINDER_TFNOVA;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_TF_TASK_PERIOD_MS));
|
||||
if (lidarTFDetect(dev, rangefinderHardwareToUse)) {
|
||||
rangefinderHardware = rangefinderHardwareToUse;
|
||||
rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(dev->delayMs));
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue