From a510091bf154851c67f0c8b9742adda0a8143381 Mon Sep 17 00:00:00 2001 From: jflyper Date: Sat, 16 Dec 2017 15:09:58 +0900 Subject: [PATCH 1/2] Add Benewake TFmini/TF02 support --- make/source.mk | 1 + src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + .../drivers/rangefinder/rangefinder_lidartf.c | 276 ++++++++++++++++++ .../drivers/rangefinder/rangefinder_lidartf.h | 25 ++ src/main/interface/settings.c | 2 +- src/main/io/serial.h | 1 + src/main/sensors/rangefinder.c | 21 +- src/main/sensors/rangefinder.h | 11 +- src/main/target/OMNIBUSF4/target.h | 1 + 10 files changed, 334 insertions(+), 6 deletions(-) create mode 100644 src/main/drivers/rangefinder/rangefinder_lidartf.c create mode 100644 src/main/drivers/rangefinder/rangefinder_lidartf.h diff --git a/make/source.mk b/make/source.mk index e39cf4f4b1..1c19992c9a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -144,6 +144,7 @@ FC_SRC = \ drivers/display_ug2864hsweg01.c \ drivers/light_ws2811strip.c \ drivers/rangefinder/rangefinder_hcsr04.c \ + drivers/rangefinder/rangefinder_lidartf.c \ drivers/serial_escserial.c \ drivers/vtx_common.c \ flight/navigation.c \ diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 0c1df15c7a..6f3a94a1b0 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -58,4 +58,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "FPORT", "RANGEFINDER", "RANGEFINDER_QUALITY", + "LIDAR_TF", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 5640a2b3f7..41960319e0 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -76,6 +76,7 @@ typedef enum { DEBUG_FPORT, DEBUG_RANGEFINDER, DEBUG_RANGEFINDER_QUALITY, + DEBUG_LIDAR_TF, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.c b/src/main/drivers/rangefinder/rangefinder_lidartf.c new file mode 100644 index 0000000000..572a4c2d3f --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_lidartf.c @@ -0,0 +1,276 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_RANGEFINDER_TF + +#include "build/debug.h" +#include "build/build_config.h" + +#include "io/serial.h" + +#include "drivers/time.h" +#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 + +static uint8_t tfDevtype = TF_DEVTYPE_NONE; + +#define TF_FRAME_LENGTH 6 // Exclusing sync bytes (0x59) x 2 and checksum +#define TF_FRAME_SYNC_BYTE 0x59 +#define TF_TIMEOUT_MS (100 * 2) + +// +// Benewake TFmini frame format +// Byte +// -: SYNC +// -: SYNC +// 0: Measured distance (LSB) +// 1: Measured distance (MSB) +// 2: Signal strength (LSB) +// 3: Signal strength (MSB) +// 4: Integral time +// 5: Reserved +// -: Checksum (Unsigned 8-bit sum of bytes 0~7) +// +// Note *1: TFmini product specification (Version A00) specifies byte 6 is +// reserved and byte 7 is quality, but it seems like byte 7 contains +// value of 7 for good measurement and 2 for bad measurement. +// +// Credibility +// 1. If distance is 12m (1200cm), then OoR. +// +#define TF_MINI_FRAME_INTEGRAL_TIME 4 +#define TF_MINI_RANGE_MIN 40 +#define TF_MINI_RANGE_MAX 1200 +#define TF_DETECTION_CONE_DECIDEGREES 900 // Very conservative. + +// +// Benewake TF02 frame format (From SJ-GU-TF02-01 Version: A01) +// Byte +// -: SYNC +// -: SYNC +// 0: Measured distance (LSB) +// 1: Measured distance (MSB) +// 2: Signal strength (LSB) +// 3: Signal strength (MSB) +// 4: SIG (Reliability in 1~8, less than 7 is unreliable) +// 5: TIME (Exposure time, 3 or 6) +// -: Checksum (Unsigned 8-bit sum of bytes 0~7) +// +// Credibility +// 1. If SIG is less than 7, unreliable +// 2. If distance is 22m (2200cm), then OoR. +// +#define TF_02_FRAME_SIG 4 +#define TF_02_RANGE_MIN 40 +#define TF_02_RANGE_MAX 2200 +#define TF_02_DETECTION_CONE_DECIDEGREES 45 + +static serialPort_t *tfSerialPort = NULL; + +typedef enum { + TF_FRAME_STATE_WAIT_START1, + TF_FRAME_STATE_WAIT_START2, + TF_FRAME_STATE_READING_PAYLOAD, + TF_FRAME_STATE_WAIT_CKSUM, +} tfFrameState_e; + +static tfFrameState_e tfFrameState; +static uint8_t tfFrame[TF_FRAME_LENGTH]; +static uint8_t tfReceivePosition; + +// TFmini +// 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 int32_t lidarTFValue; +static uint16_t lidarTFerrors = 0; + +static void lidarTFSendCommand(void) +{ + switch (tfDevtype) { + case TF_DEVTYPE_MINI: + serialWriteBuf(tfSerialPort, tfCmdTFmini, ARRAYLEN(tfCmdTFmini)); + break; + case TF_DEVTYPE_02: + serialWriteBuf(tfSerialPort, tfCmdTF02, ARRAYLEN(tfCmdTF02)); + break; + } +} + +void lidarTFInit(rangefinderDev_t *dev) +{ + UNUSED(dev); + + tfFrameState = TF_FRAME_STATE_WAIT_START1; + tfReceivePosition = 0; +} + +void lidarTFUpdate(rangefinderDev_t *dev) +{ + UNUSED(dev); + static timeMs_t lastFrameReceivedMs = 0; + const timeMs_t timeNowMs = millis(); + + if (tfSerialPort == NULL) { + return; + } + + while (serialRxBytesWaiting(tfSerialPort)) { + uint8_t c = serialRead(tfSerialPort); + switch (tfFrameState) { + case TF_FRAME_STATE_WAIT_START1: + if (c == TF_FRAME_SYNC_BYTE) { + tfFrameState = TF_FRAME_STATE_WAIT_START2; + } + break; + + case TF_FRAME_STATE_WAIT_START2: + if (c == TF_FRAME_SYNC_BYTE) { + tfFrameState = TF_FRAME_STATE_READING_PAYLOAD; + } else { + tfFrameState = TF_FRAME_STATE_WAIT_START1; + } + break; + + case TF_FRAME_STATE_READING_PAYLOAD: + tfFrame[tfReceivePosition++] = c; + if (tfReceivePosition == TF_FRAME_LENGTH) { + tfFrameState = TF_FRAME_STATE_WAIT_CKSUM; + } + 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; + } + lastFrameReceivedMs = timeNowMs; + } else { + // Checksum error. Simply discard the current frame. + ++lidarTFerrors; + //DEBUG_SET(DEBUG_LIDAR_TF, 3, 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(); + } +} + +// Return most recent device output in cm + +int32_t lidarTFGetDistance(rangefinderDev_t *dev) +{ + UNUSED(dev); + + return lidarTFValue; +} + +static bool lidarTFDetect(rangefinderDev_t *dev, uint8_t devtype) +{ + 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; + dev->maxRangeCm = (devtype == TF_DEVTYPE_MINI) ? TF_MINI_RANGE_MAX : TF_02_RANGE_MAX; + dev->detectionConeDeciDegrees = TF_DETECTION_CONE_DECIDEGREES; + dev->detectionConeExtendedDeciDegrees = TF_DETECTION_CONE_DECIDEGREES; + + dev->init = &lidarTFInit; + dev->update = &lidarTFUpdate; + dev->read = &lidarTFGetDistance; + + return true; +} + +bool lidarTFminiDetect(rangefinderDev_t *dev) +{ + return lidarTFDetect(dev, TF_DEVTYPE_MINI); +} + +bool lidarTF02Detect(rangefinderDev_t *dev) +{ + return lidarTFDetect(dev, TF_DEVTYPE_02); +} +#endif diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.h b/src/main/drivers/rangefinder/rangefinder_lidartf.h new file mode 100644 index 0000000000..b6e94617eb --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_lidartf.h @@ -0,0 +1,25 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include "common/time.h" + +#define RANGEFINDER_TF_TASK_PERIOD_MS 10 + +bool lidarTFminiDetect(rangefinderDev_t *dev); +bool lidarTF02Detect(rangefinderDev_t *dev); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index ad8fb541a2..b389756254 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -112,7 +112,7 @@ const char * const lookupTableMagHardware[] = { #endif #if defined(USE_SENSOR_NAMES) || defined(USE_RANGEFINDER) const char * const lookupTableRangefinderHardware[] = { - "NONE", "HCSR04" + "NONE", "HCSR04", "TFMINI", "TF02" }; #endif diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 22377320be..068b91808e 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -45,6 +45,7 @@ typedef enum { FUNCTION_TELEMETRY_IBUS = (1 << 12), // 4096 FUNCTION_VTX_TRAMP = (1 << 13), // 8192 FUNCTION_RCDEVICE = (1 << 14), // 16384 + FUNCTION_LIDAR_TF = (1 << 15), // 32768 } serialPortFunction_e; typedef enum { diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index f3c8b32d19..d1de908b71 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -37,8 +37,9 @@ #include "drivers/io.h" #include "drivers/time.h" -#include "drivers/rangefinder/rangefinder_hcsr04.h" #include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_hcsr04.h" +#include "drivers/rangefinder/rangefinder_lidartf.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -143,6 +144,24 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + 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_NONE: rangefinderHardware = RANGEFINDER_NONE; break; diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 183d40a07a..1ef961be8a 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -24,10 +24,13 @@ typedef enum { RANGEFINDER_NONE = 0, RANGEFINDER_HCSR04 = 1, - RANGEFINDER_SRF10 = 2, - RANGEFINDER_HCSR04I2C = 3, - RANGEFINDER_VL53L0X = 4, - RANGEFINDER_UIB = 5, + RANGEFINDER_TFMINI = 2, + RANGEFINDER_TF02 = 3, + // Following sensors are to be ported from iNav + RANGEFINDER_SRF10 = 4, + RANGEFINDER_HCSR04I2C = 5, + RANGEFINDER_VL53L0X = 6, + RANGEFINDER_UIB = 7, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 21c856a5cc..4b99833006 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -247,6 +247,7 @@ #define USE_RANGEFINDER_HCSR04 #define RANGEFINDER_HCSR04_TRIGGER_PIN PA1 #define RANGEFINDER_HCSR04_ECHO_PIN PA8 +#define USE_RANGEFINDER_TF #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL From 59a7510488770d94a36b18c66f69ba70609cc91f Mon Sep 17 00:00:00 2001 From: jflyper Date: Thu, 21 Dec 2017 22:17:22 +0900 Subject: [PATCH 2/2] Amended comment for frame format + cosmetic changes --- .../drivers/rangefinder/rangefinder_lidartf.c | 62 +++++++++---------- 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_lidartf.c b/src/main/drivers/rangefinder/rangefinder_lidartf.c index 572a4c2d3f..367caedba6 100644 --- a/src/main/drivers/rangefinder/rangefinder_lidartf.c +++ b/src/main/drivers/rangefinder/rangefinder_lidartf.c @@ -37,56 +37,56 @@ static uint8_t tfDevtype = TF_DEVTYPE_NONE; -#define TF_FRAME_LENGTH 6 // Exclusing sync bytes (0x59) x 2 and checksum +#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) // // Benewake TFmini frame format -// Byte -// -: SYNC -// -: SYNC -// 0: Measured distance (LSB) -// 1: Measured distance (MSB) -// 2: Signal strength (LSB) -// 3: Signal strength (MSB) -// 4: Integral time -// 5: Reserved -// -: Checksum (Unsigned 8-bit sum of bytes 0~7) -// -// Note *1: TFmini product specification (Version A00) specifies byte 6 is -// reserved and byte 7 is quality, but it seems like byte 7 contains -// value of 7 for good measurement and 2 for bad measurement. +// 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 Integral time +// 8 5 Reserved +// 9 - Checksum (Unsigned 8-bit sum of bytes 0~7) // // Credibility // 1. If distance is 12m (1200cm), then OoR. // #define TF_MINI_FRAME_INTEGRAL_TIME 4 -#define TF_MINI_RANGE_MIN 40 -#define TF_MINI_RANGE_MAX 1200 -#define TF_DETECTION_CONE_DECIDEGREES 900 // Very conservative. // // Benewake TF02 frame format (From SJ-GU-TF02-01 Version: A01) -// Byte -// -: SYNC -// -: SYNC -// 0: Measured distance (LSB) -// 1: Measured distance (MSB) -// 2: Signal strength (LSB) -// 3: Signal strength (MSB) -// 4: SIG (Reliability in 1~8, less than 7 is unreliable) -// 5: TIME (Exposure time, 3 or 6) -// -: Checksum (Unsigned 8-bit sum of bytes 0~7) +// 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 SIG (Reliability in 1~8, less than 7 is unreliable) +// 8 5 TIME (Exposure time, 3 or 6) +// 9 - Checksum (Unsigned 8-bit sum of bytes 0~7) // // Credibility // 1. If SIG is less than 7, unreliable // 2. If distance is 22m (2200cm), then OoR. // #define TF_02_FRAME_SIG 4 + +// 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_02_DETECTION_CONE_DECIDEGREES 45 + +#define TF_DETECTION_CONE_DECIDEGREES 900 static serialPort_t *tfSerialPort = NULL; @@ -117,10 +117,10 @@ static void lidarTFSendCommand(void) { switch (tfDevtype) { case TF_DEVTYPE_MINI: - serialWriteBuf(tfSerialPort, tfCmdTFmini, ARRAYLEN(tfCmdTFmini)); + serialWriteBuf(tfSerialPort, tfCmdTFmini, sizeof(tfCmdTFmini)); break; case TF_DEVTYPE_02: - serialWriteBuf(tfSerialPort, tfCmdTF02, ARRAYLEN(tfCmdTF02)); + serialWriteBuf(tfSerialPort, tfCmdTF02, sizeof(tfCmdTF02)); break; } }