rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #4793 from jflyper/bfdev-rangefinder-benewake-new 

RANGEFINDER Add Benewake TFmini/TF02 support
This commit is contained in:
Michael Keller 2017-12-22 18:11:03 +13:00 committed by GitHub
parent 12ef70d5f0 59a7510488
commit a618de9e8e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
10 changed files with 334 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
make/source.mk
View File

@ -145,6 +145,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 \

1
src/main/build/debug.c
View File

@ -58,4 +58,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"FPORT",
"RANGEFINDER",
"RANGEFINDER_QUALITY",
"LIDAR_TF",
};

1
src/main/build/debug.h
View File

@ -76,6 +76,7 @@ typedef enum {
DEBUG_FPORT,
DEBUG_RANGEFINDER,
DEBUG_RANGEFINDER_QUALITY,
DEBUG_LIDAR_TF,
DEBUG_COUNT
} debugType_e;

276
src/main/drivers/rangefinder/rangefinder_lidartf.c Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#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 // 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 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
//
// Benewake TF02 frame format (From SJ-GU-TF02-01 Version: A01)
// 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_DETECTION_CONE_DECIDEGREES 900
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, sizeof(tfCmdTFmini));
break;
case TF_DEVTYPE_02:
serialWriteBuf(tfSerialPort, tfCmdTF02, sizeof(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

25
src/main/drivers/rangefinder/rangefinder_lidartf.h Normal file
View File

@ -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 <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "common/time.h"
#define RANGEFINDER_TF_TASK_PERIOD_MS 10
bool lidarTFminiDetect(rangefinderDev_t *dev);
bool lidarTF02Detect(rangefinderDev_t *dev);

2
src/main/interface/settings.c
View File

@ -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

1
src/main/io/serial.h
View File

@ -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 {

21
src/main/sensors/rangefinder.c
View File

@ -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;

11
src/main/sensors/rangefinder.h
View File

@ -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 {

1
src/main/target/OMNIBUSF4/target.h
View File

@ -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