Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_RangeFinder: reduce verbiage when detecting serial rangefinders

This commit is contained in:
Peter Barker 2022-06-22 21:03:22 +10:00 committed by Peter Barker
parent 55fb6c2c51
commit ffc4910c7e
17 changed files with 133 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
libraries/AP_RangeFinder/AP_RangeFinder.cpp
View File

@ -343,6 +343,8 @@ bool RangeFinder::_add_backend(AP_RangeFinder_Backend *backend, uint8_t instance
void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
{
#if AP_RANGEFINDER_ENABLED
AP_RangeFinder_Backend_Serial *(*serial_create_fn)(RangeFinder::RangeFinder_State&, AP_RangeFinder_Params&) = nullptr;
const Type _type = (Type)params[instance].type.get();
switch (_type) {
case Type::PLI2C:
@ -464,23 +466,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
case Type::LWSER:
#if AP_RANGEFINDER_LIGHTWARE_SERIAL_ENABLED
if (AP_RangeFinder_LightWareSerial::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_LightWareSerial(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_LightWareSerial::create;
#endif
break;
case Type::LEDDARONE:
#if AP_RANGEFINDER_LEDDARONE_ENABLED
if (AP_RangeFinder_LeddarOne::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_LeddarOne(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_LeddarOne::create;
#endif
break;
case Type::USD1_Serial:
#if AP_RANGEFINDER_USD1_SERIAL_ENABLED
if (AP_RangeFinder_USD1_Serial::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_USD1_Serial(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_USD1_Serial::create;
#endif
break;
case Type::BEBOP:
@ -499,9 +495,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
case Type::MBSER:
#if AP_RANGEFINDER_MAXBOTIX_SERIAL_ENABLED
if (AP_RangeFinder_MaxsonarSerialLV::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_MaxsonarSerialLV(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_MaxsonarSerialLV::create;
#endif
break;
case Type::ANALOG:
@ -522,37 +516,27 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
case Type::NMEA:
#if AP_RANGEFINDER_NMEA_ENABLED
if (AP_RangeFinder_NMEA::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_NMEA(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_NMEA::create;
#endif
break;
case Type::WASP:
#if AP_RANGEFINDER_WASP_ENABLED
if (AP_RangeFinder_Wasp::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_Wasp(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_Wasp::create;
#endif
break;
case Type::BenewakeTF02:
#if AP_RANGEFINDER_BENEWAKE_TF02_ENABLED
if (AP_RangeFinder_Benewake_TF02::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_Benewake_TF02(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_Benewake_TF02::create;
#endif
break;
case Type::BenewakeTFmini:
#if AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED
if (AP_RangeFinder_Benewake_TFMini::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_Benewake_TFMini(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_Benewake_TFMini::create;
#endif
break;
case Type::BenewakeTF03:
#if AP_RANGEFINDER_BENEWAKE_TF03_ENABLED
if (AP_RangeFinder_Benewake_TF03::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_Benewake_TF03(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_Benewake_TF03::create;
#endif
break;
case Type::PWM:
@ -564,23 +548,17 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
case Type::BLPing:
#if AP_RANGEFINDER_BLPING_ENABLED
if (AP_RangeFinder_BLPing::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_BLPing(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_BLPing::create;
#endif
break;
case Type::Lanbao:
#if AP_RANGEFINDER_LANBAO_ENABLED
if (AP_RangeFinder_Lanbao::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_Lanbao(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_Lanbao::create;
#endif
break;
case Type::LeddarVu8_Serial:
#if AP_RANGEFINDER_LEDDARVU8_ENABLED
if (AP_RangeFinder_LeddarVu8::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_LeddarVu8(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_LeddarVu8::create;
#endif
break;
@ -597,9 +575,7 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
case Type::GYUS42v2:
#if AP_RANGEFINDER_GYUS42V2_ENABLED
if (AP_RangeFinder_GYUS42v2::detect(serial_instance)) {
_add_backend(new AP_RangeFinder_GYUS42v2(state[instance], params[instance]), instance, serial_instance++);
}
serial_create_fn = AP_RangeFinder_GYUS42v2::create;
#endif
break;
@ -631,6 +607,15 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
break;
}
if (serial_create_fn != nullptr) {
if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance)) {
auto *b = serial_create_fn(state[instance], params[instance]);
if (b != nullptr) {
_add_backend(b, instance, serial_instance++);
}
}
}
// if the backend has some local parameters then make those available in the tree
if (drivers[instance] && state[instance].var_info) {
backend_var_info[instance] = state[instance].var_info;

10
libraries/AP_RangeFinder/AP_RangeFinder_BLPing.h
View File

@ -124,7 +124,12 @@ class AP_RangeFinder_BLPing : public AP_RangeFinder_Backend_Serial
static constexpr uint16_t _sensor_rate_ms = 50; // initialise sensor at no more than 20hz
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_BLPing(_state, _params);
}
/**
* @brief Update class state
@ -149,6 +154,9 @@ protected:
PingProtocol protocol;
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
/**
* @brief Do the necessary sensor initiation
*

10
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.cpp
View File

@ -47,16 +47,6 @@ uint32_t AP_RangeFinder_Backend_Serial::initial_baudrate(const uint8_t serial_in
return AP::serialmanager().find_baudrate(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
}
/*
detect if a Serial rangefinder is connected. We'll detect by simply
checking for SerialManager configuration
*/
bool AP_RangeFinder_Backend_Serial::detect(uint8_t serial_instance)
{
return AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_Rangefinder, serial_instance);
}
/*
update the state of the sensor
*/

2
libraries/AP_RangeFinder/AP_RangeFinder_Backend_Serial.h
View File

@ -10,8 +10,6 @@ public:
AP_RangeFinder_Params &_params);
void init_serial(uint8_t serial_instance) override;
// static detection function
static bool detect(uint8_t serial_instance);
protected:

12
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h
View File

@ -11,11 +11,21 @@
class AP_RangeFinder_Benewake_TF02 : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_Benewake_TF02(_state, _params);
}
protected:
float model_dist_max_cm() const override { return 2200; }
bool has_signal_byte() const override { return true; }
private:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
};
#endif // AP_RANGEFINDER_BENEWAKE_TF02_ENABLED

10
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h
View File

@ -11,10 +11,18 @@
class AP_RangeFinder_Benewake_TF03 : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_Benewake_TF03(_state, _params);
}
protected:
float model_dist_max_cm() const override { return 18000; }
private:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
};
#endif // AP_RANGEFINDER_BENEWAKE_TF03_ENABLED

11
libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h
View File

@ -11,10 +11,19 @@
class AP_RangeFinder_Benewake_TFMini : public AP_RangeFinder_Benewake
{
public:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_Benewake_TFMini(_state, _params);
}
protected:
float model_dist_max_cm() const override { return 1200; }
private:
using AP_RangeFinder_Benewake::AP_RangeFinder_Benewake;
};
#endif // AP_RANGEFINDER_BENEWAKE_TFMINI

8
libraries/AP_RangeFinder/AP_RangeFinder_GYUS42v2.h
View File

@ -15,7 +15,11 @@ class AP_RangeFinder_GYUS42v2 : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_GYUS42v2(_state, _params);
}
protected:
@ -29,6 +33,8 @@ protected:
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// get a reading
bool get_reading(float &reading_m) override;

9
libraries/AP_RangeFinder/AP_RangeFinder_Lanbao.h
View File

@ -14,7 +14,11 @@ class AP_RangeFinder_Lanbao : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_Lanbao(_state, _params);
}
// Lanbao is always 115200:
uint32_t initial_baudrate(uint8_t serial_instance) const override {
@ -28,6 +32,9 @@ protected:
}
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// get a reading
bool get_reading(float &reading_m) override;

9
libraries/AP_RangeFinder/AP_RangeFinder_LeddarOne.h
View File

@ -48,7 +48,11 @@ class AP_RangeFinder_LeddarOne : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_LeddarOne(_state, _params);
}
protected:
@ -57,6 +61,9 @@ protected:
}
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// get a reading
bool get_reading(float &reading_m) override;

8
libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h
View File

@ -16,7 +16,11 @@ class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_LeddarVu8(_state, _params);
}
protected:
@ -38,6 +42,8 @@ protected:
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// function codes
enum class FunctionCode : uint8_t {
READ_HOLDING_REGISTER = 0x03,

8
libraries/AP_RangeFinder/AP_RangeFinder_LightWareSerial.h
View File

@ -14,10 +14,16 @@ class AP_RangeFinder_LightWareSerial : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_LightWareSerial(_state, _params);
}
protected:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
return MAV_DISTANCE_SENSOR_LASER;
}

1
libraries/AP_RangeFinder/AP_RangeFinder_MAVLink.h
View File

@ -16,6 +16,7 @@ class AP_RangeFinder_MAVLink : public AP_RangeFinder_Backend
{
public:
// constructor
using AP_RangeFinder_Backend::AP_RangeFinder_Backend;

9
libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
View File

@ -14,7 +14,11 @@ class AP_RangeFinder_MaxsonarSerialLV : public AP_RangeFinder_Backend_Serial
public:
AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_MaxsonarSerialLV(_state, _params);
}
protected:
@ -23,6 +27,9 @@ protected:
}
private:
AP_RangeFinder_MaxsonarSerialLV(RangeFinder::RangeFinder_State &_state, AP_RangeFinder_Params &_params);
// get a reading
bool get_reading(float &reading_m) override;

8
libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h
View File

@ -29,7 +29,11 @@ class AP_RangeFinder_NMEA : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_NMEA(_state, _params);
}
protected:
@ -39,6 +43,8 @@ protected:
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
/// enum for handled messages
enum sentence_types : uint8_t {
SONAR_UNKNOWN = 0,

9
libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.h
View File

@ -14,7 +14,11 @@ class AP_RangeFinder_USD1_Serial : public AP_RangeFinder_Backend_Serial
public:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_USD1_Serial(_state, _params);
}
protected:
@ -31,6 +35,9 @@ protected:
uint16_t tx_bufsize() const override { return 128; }
private:
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
// detect USD1_Serial Firmware Version
bool detect_version(void);

11
libraries/AP_RangeFinder/AP_RangeFinder_Wasp.h
View File

@ -15,8 +15,12 @@
class AP_RangeFinder_Wasp : public AP_RangeFinder_Backend_Serial {
public:
AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params);
static AP_RangeFinder_Backend_Serial *create(
RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params) {
return new AP_RangeFinder_Wasp(_state, _params);
}
void update(void) override;
@ -35,6 +39,9 @@ protected:
private:
AP_RangeFinder_Wasp(RangeFinder::RangeFinder_State &_state,
AP_RangeFinder_Params &_params);
enum wasp_configuration_stage {
WASP_CFG_RATE, // set the baudrate
WASP_CFG_ENCODING, // set the encoding to LBE