aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_RangeFinder: make Benewake backend use new intermediate class

Browse Source
This commit is contained in:
Peter Barker 2019-11-01 18:03:17 +11:00 committed by Peter Barker
parent 0246dd990e
commit acc1f955ef
7 changed files with 60 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
View File

@ -13,20 +13,18 @@
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/AP_HAL.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <ctype.h>
extern const AP_HAL::HAL& hal;
#define BENEWAKE_FRAME_HEADER 0x59
#define BENEWAKE_FRAME_LENGTH 9
#define BENEWAKE_DIST_MAX_CM 32768
#define BENEWAKE_TFMINI_OUT_OF_RANGE_CM 1200
#define BENEWAKE_TF02_OUT_OF_RANGE_CM 2200
#define BENEWAKE_TF03_OUT_OF_RANGE_CM 18000
#define BENEWAKE_OUT_OF_RANGE_ADD_CM 100
// format of serial packets received from benewake lidar
@ -47,35 +45,6 @@ extern const AP_HAL::HAL& hal;
// 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_RangeFinder_Params &_params,
uint8_t serial_instance,
benewake_model_type model) :
AP_RangeFinder_Backend(_state, _params),
model_type(model)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
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(uint8_t serial_instance)
{
return AP::serialmanager().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)
{
@ -125,7 +94,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
if (dist >= BENEWAKE_DIST_MAX_CM) {
// this reading is out of range
count_out_of_range++;
} else if (model_type == BENEWAKE_TFmini || model_type == BENEWAKE_TF03) {
} else if (!has_signal_byte()) {
// no signal byte from TFmini so add distance to sum
sum_cm += dist;
count++;
@ -156,19 +125,7 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
if (count_out_of_range > 0) {
// if only out of range readings return larger of
// driver defined maximum range for the model and user defined max range + 1m
float model_dist_max_cm = 0.0f;
switch (model_type) {
case BENEWAKE_TFmini:
model_dist_max_cm = BENEWAKE_TFMINI_OUT_OF_RANGE_CM;
break;
case BENEWAKE_TF02:
model_dist_max_cm = BENEWAKE_TF02_OUT_OF_RANGE_CM;
break;
case BENEWAKE_TF03:
model_dist_max_cm = BENEWAKE_TF03_OUT_OF_RANGE_CM;
break;
}
reading_cm = MAX(model_dist_max_cm, max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
reading_cm = MAX(model_dist_max_cm(), max_distance_cm() + BENEWAKE_OUT_OF_RANGE_ADD_CM);
return true;
}

24
libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
View File

@ -1,27 +1,14 @@
#pragma once
#include "RangeFinder.h"
#include "RangeFinder_Backend.h"
#include "RangeFinder_Backend_Serial.h"
class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend
class AP_RangeFinder_Benewake : public AP_RangeFinder_Backend_Serial
{
public:
enum benewake_model_type {
BENEWAKE_TF02 = 0,
BENEWAKE_TFmini = 1,
BENEWAKE_TF03 = 2,
};
// constructor
AP_RangeFinder_Benewake(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params,
uint8_t serial_instance,
benewake_model_type model);
// static detection function
static bool detect(uint8_t serial_instance);
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// update state
void update(void) override;
@ -32,14 +19,15 @@ protected:
return MAV_DISTANCE_SENSOR_LASER;
}
virtual float model_dist_max_cm() const = 0;
virtual bool has_signal_byte() const { return false; }
private:
// get a reading
// distance returned in reading_cm
bool get_reading(uint16_t &reading_cm);
AP_HAL::UARTDriver *uart = nullptr;
benewake_model_type model_type;
uint8_t linebuf[10];
uint8_t linebuf_len;
};

13
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h Normal file
View File

@ -0,0 +1,13 @@
#pragma once
#include "AP_RangeFinder_Benewake.h"
class AP_RangeFinder_Benewake_TF02 : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
protected:
float model_dist_max_cm() const override { return 2200; }
bool has_signal_byte() const override { return true; }
};

12
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h Normal file
View File

@ -0,0 +1,12 @@
#pragma once
#include "AP_RangeFinder_Benewake.h"
class AP_RangeFinder_Benewake_TF03 : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
protected:
float model_dist_max_cm() const override { return 18000; }
};

12
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h Normal file
View File

@ -0,0 +1,12 @@
#pragma once
#include "AP_RangeFinder_Benewake.h"
class AP_RangeFinder_Benewake_TFMini : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
protected:
float model_dist_max_cm() const override { return 1200; }
};

2
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -8,6 +8,8 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// update state
void update(void) override;

16
libraries/AP_RangeFinder/RangeFinder.cpp
View File

@ -34,7 +34,9 @@
#include "AP_RangeFinder_VL53L1X.h"
#include "AP_RangeFinder_NMEA.h"
#include "AP_RangeFinder_Wasp.h"
#include "AP_RangeFinder_Benewake.h"
#include "AP_RangeFinder_Benewake_TF02.h"
#include "AP_RangeFinder_Benewake_TF03.h"
#include "AP_RangeFinder_Benewake_TFMini.h"
#include "AP_RangeFinder_Benewake_TFMiniPlus.h"
#include "AP_RangeFinder_PWM.h"
#include "AP_RangeFinder_BLPing.h"
@ -500,18 +502,18 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
}
break;
case Type::BenewakeTF02:
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF02);
if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TF02(state[instance], params[instance], serial_instance++);
}
break;
case Type::BenewakeTFmini:
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TFmini);
if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance], serial_instance++);
}
break;
case Type::BenewakeTF03:
if (AP_RangeFinder_Benewake::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake(state[instance], params[instance], serial_instance++, AP_RangeFinder_Benewake::BENEWAKE_TF03);
if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) {
drivers[instance] = new AP_RangeFinder_Benewake_TF03(state[instance], params[instance], serial_instance++);
}
break;
case Type::PWM: