Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: Benewake TF02 and TFmini lidar driver

Browse Source
This commit is contained in:
Randy Mackay 2018-05-26 10:59:36 +09:00
parent 2c41db9e4d
commit e520d4fe42
4 changed files with 227 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

165
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp Normal file
View File

@ -0,0 +1,165 @@
/*
This program 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.
This program 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 this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include <AP_HAL/AP_HAL.h>
#include "AP_RangeFinder_Benewake.h"
#include <AP_SerialManager/AP_SerialManager.h>
#include <ctype.h>
#include <AP_HAL/utility/sparse-endian.h>
extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9
// format of serial packets received from benewake lidar
//
// Data Bit Definition Description
// ------------------------------------------------
// byte 0 Frame header 0x59
// byte 1 Frame header 0x59
// byte 2 DIST_L Distance (in cm) low 8 bits
// byte 3 DIST_H Distance (in cm) high 8 bits
// byte 4 STRENGTH_L Strength low 8 bits
// byte 5 STRENGTH_H Strength high 8 bits
// byte 6 (TF02) SIG Reliability in 8 levels, 7 & 8 means reliable
// byte 6 (TFmini) Distance Mode 0x02 for short distance (mm), 0x07 for long distance (cm)
// byte 7 (TF02 only) TIME Exposure time in two levels 0x03 and 0x06
// byte 8 Checksum Checksum byte, sum of bytes 0 to bytes 7
/*
The constructor also initialises the rangefinder. Note that this
constructor is not called until detect() returns true, so we
already know that we should setup the rangefinder
*/
AP_RangeFinder_Benewake::AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model) :
AP_RangeFinder_Backend(_state),
model_type(model)
{
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
if (uart != nullptr) {
uart->begin(serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance));
}
}
/*
detect if a Benewake rangefinder is connected. We'll detect by
trying to take a reading on Serial. If we get a result the sensor is
there.
*/
bool AP_RangeFinder_Benewake::detect(AP_SerialManager &serial_manager, uint8_t serial_instance)
{
return serial_manager.find_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance) != nullptr;
}
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm, bool &signal_ok)
{
if (uart == nullptr) {
return false;
}
float sum_cm = 0;
uint16_t count = 0;
bool dist_reliable = false;
// read any available lines from the lidar
int16_t nbytes = uart->available();
while (nbytes-- > 0) {
char c = uart->read();
// if buffer is empty and this byte is 0x59, add to buffer
if (linebuf_len == 0) {
if (c == BENEWAKE_FRAME_HEADER) {
linebuf[linebuf_len++] = c;
}
} else if (linebuf_len == 1) {
// if buffer has 1 element and this byte is 0x59, add it to buffer
// if not clear the buffer
if (c == BENEWAKE_FRAME_HEADER) {
linebuf[linebuf_len++] = c;
} else {
linebuf_len = 0;
dist_reliable = false;
}
} else {
// add character to buffer
linebuf[linebuf_len++] = c;
// if buffer now has 9 items try to decode it
if (linebuf_len == BENEWAKE_FRAME_LENGTH) {
// calculate checksum
uint16_t checksum = 0;
for (uint8_t i=0; i<BENEWAKE_FRAME_LENGTH-1; i++) {
checksum += linebuf[i];
}
// if checksum matches extract contents
if ((uint8_t)(checksum & 0xFF) == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
// tell caller we are receiving packets
signal_ok = true;
// calculate distance and add to sum
uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
if (dist != 0xFFFF) {
// TFmini has short distance mode (mm)
if ((model_type == BENEWAKE_TFmini)) {
if (linebuf[6] == 0x02) {
dist *= 0.1f;
}
// no signal byte from TFmini
dist_reliable = true;
} else {
// TF02 provides signal reliability (good = 7 or 8)
dist_reliable = (linebuf[6] >= 7);
}
if (dist_reliable) {
sum_cm += dist;
count++;
}
}
}
// clear buffer
linebuf_len = 0;
dist_reliable = false;
}
}
}
if (count == 0) {
return false;
}
reading_cm = sum_cm / count;
return true;
}
/*
update the state of the sensor
*/
void AP_RangeFinder_Benewake::update(void)
{
bool signal_ok;
if (get_reading(state.distance_cm, signal_ok)) {
// update range_valid state based on distance measured
last_reading_ms = AP_HAL::millis();
if (signal_ok) {
update_status();
} else {
// if signal is weak set status to out-of-range
set_status(RangeFinder::RangeFinder_OutOfRangeHigh);
}
} else if (AP_HAL::millis() - last_reading_ms > 200) {
set_status(RangeFinder::RangeFinder_NoData);
}
}

45
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h Normal file
View File

@ -0,0 +1,45 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend
{
public:
enum benewake_model_type {
BENEWAKE_TF02 = 0,
BENEWAKE_TFmini = 1
};
// constructor
AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_SerialManager &serial_manager,
uint8_t serial_instance,
benewake_model_type model);
// static detection function
static bool detect(AP_SerialManager &serial_manager, uint8_t serial_instance);
// update state
void update(void);
protected:
virtual MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_LASER;
}
private:
// get a reading
// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
bool get_reading(uint16_t &reading_cm, bool &signal_ok);
AP_HAL::UARTDriver *uart = nullptr;
benewake_model_type model_type;
uint32_t last_reading_ms;
char linebuf[10];
uint8_t linebuf_len;
};

19
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -34,6 +34,7 @@
#include "AP_RangeFinder_VL53L0X.h"
#include "AP_RangeFinder_NMEA.h"
#include "AP_RangeFinder_Wasp.h"
#include "AP_RangeFinder_Benewake.h"
#include <AP_BoardConfig/AP_BoardConfig.h>
extern const AP_HAL::HAL &hal;
@ -43,7 +44,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: _TYPE
// @DisplayName: Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200,19:BenewakeTF02,20:BenewakeTFmini
// @User: Standard
AP_GROUPINFO("_TYPE", 0, RangeFinder, state[0].type, 0),
@ -174,7 +175,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 2_TYPE
// @DisplayName: Second Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200,19:BenewakeTF02,20:BenewakeTFmini
// @User: Advanced
AP_GROUPINFO("2_TYPE", 12, RangeFinder, state[1].type, 0),
@ -299,7 +300,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 3_TYPE
// @DisplayName: Third Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200,19:BenewakeTF02,20:BenewakeTFmini
// @User: Advanced
AP_GROUPINFO("3_TYPE", 25, RangeFinder, state[2].type, 0),
@ -424,7 +425,7 @@ const AP_Param::GroupInfo RangeFinder::var_info[] = {
// @Param: 4_TYPE
// @DisplayName: Fourth Rangefinder type
// @Description: What type of rangefinder device that is connected
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200
// @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLiteV2-I2C,5:PX4-PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-200,19:BenewakeTF02,20:BenewakeTFmini
// @User: Advanced
AP_GROUPINFO("4_TYPE", 37, RangeFinder, state[3].type, 0),
@ -746,6 +747,16 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
drivers[instance] = new AP_RangeFinder_Wasp(state[instance], serial_manager, serial_instance++);
}
break;
case RangeFinder_TYPE_BenewakeTF02:
if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02);
}
break;
case RangeFinder_TYPE_BenewakeTFmini:
if (AP_RangeFinder_Benewake::detect(serial_manager, serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], serial_manager, serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
}
break;
default:
break;
}

2
libraries/AP_RangeFinder/RangeFinder.h
View File

@ -60,6 +60,8 @@ public:
RangeFinder_TYPE_VL53L0X = 16,
RangeFinder_TYPE_NMEA = 17,
RangeFinder_TYPE_WASP = 18,
RangeFinder_TYPE_BenewakeTF02 = 19,
RangeFinder_TYPE_BenewakeTFmini = 20
};
enum RangeFinder_Function {