Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Merge pull request #5881 from iNavFlight/de_tf_lidar_fixes

Browse source 
Add Benewake TFmini initialization
This commit is contained in:
Konstantin Sharlaimov 2020-11-14 16:24:41 +00:00 • committed by GitHub
commit 91a11c8b67
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 18 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

19
src/main/io/rangefinder_benewake.c
View file

@ -55,15 +55,20 @@ typedef struct __attribute__((packed)) {
#define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t) #define BENEWAKE_PACKET_SIZE sizeof(tfminiPacket_t)
#define BENEWAKE_MIN_QUALITY 0 #define BENEWAKE_MIN_QUALITY 0
#define BENEWAKE_TIMEOUT_MS 200 // 200ms
static serialPort_t * serialPort = NULL; static serialPort_t * serialPort = NULL;
static serialPortConfig_t * portConfig; static serialPortConfig_t * portConfig;
static uint8_t buffer[BENEWAKE_PACKET_SIZE]; static uint8_t buffer[BENEWAKE_PACKET_SIZE];
static unsigned bufferPtr; static unsigned bufferPtr;
static timeMs_t lastProtocolActivityMs;
static bool hasNewData = false; static bool hasNewData = false;
static int32_t sensorData = RANGEFINDER_NO_NEW_DATA; static int32_t sensorData = RANGEFINDER_NO_NEW_DATA;
// TFmini command to initiate 100Hz sampling
static const uint8_t initCmd100Hz[] = { 0x42, 0x57, 0x02, 0x00, 0x00, 0x00, 0x01, 0x06 };
static bool benewakeRangefinderDetect(void) static bool benewakeRangefinderDetect(void)
{ {
portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER); portConfig = findSerialPortConfig(FUNCTION_RANGEFINDER);
@ -80,16 +85,27 @@ static void benewakeRangefinderInit(void)
return; return;
} }
serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, baudRates[BAUD_115200], MODE_RX, SERIAL_NOT_INVERTED); serialPort = openSerialPort(portConfig->identifier, FUNCTION_RANGEFINDER, NULL, NULL, 115200, MODE_RXTX, SERIAL_NOT_INVERTED);
if (!serialPort) { if (!serialPort) {
return; return;
} }
lastProtocolActivityMs = 0;
bufferPtr = 0; bufferPtr = 0;
} }
static void benewakeRangefinderUpdate(void) static void benewakeRangefinderUpdate(void)
{ {
// Initialize the sensor
if (lastProtocolActivityMs == 0 || (millis() - lastProtocolActivityMs) > BENEWAKE_TIMEOUT_MS) {
// Initialize the sensor to do 100Hz sampling to make sure we get the most recent data always
serialWriteBuf(serialPort, initCmd100Hz, sizeof(initCmd100Hz));
// Send the init command every BENEWAKE_TIMEOUT_MS
lastProtocolActivityMs = millis();
}
// Process incoming bytes
tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer; tfminiPacket_t *tfminiPacket = (tfminiPacket_t *)buffer;
while (serialRxBytesWaiting(serialPort) > 0) { while (serialRxBytesWaiting(serialPort) > 0) {
uint8_t c = serialRead(serialPort); uint8_t c = serialRead(serialPort);
@ -117,6 +133,7 @@ static void benewakeRangefinderUpdate(void)
// Valid packet // Valid packet
hasNewData = true; hasNewData = true;
sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8); sensorData = (tfminiPacket->distL << 0) | (tfminiPacket->distH << 8);
lastProtocolActivityMs = millis();
uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8); uint16_t qual = (tfminiPacket->strengthL << 0) | (tfminiPacket->strengthH << 8);