diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
index ae0a1f1674..6877aaba28 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp
@@ -13,20 +13,18 @@
along with this program. If not, see .
*/
-#include
#include "AP_RangeFinder_Benewake.h"
-#include
-#include
+
+#include
#include
+#include
+
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;
}
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
index 5a1b51d00f..4515c944a7 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake.h
@@ -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;
};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h
new file mode 100644
index 0000000000..89af15632c
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF02.h
@@ -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; }
+};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h
new file mode 100644
index 0000000000..38f5746fac
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TF03.h
@@ -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; }
+};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h
new file mode 100644
index 0000000000..5b3eef20b3
--- /dev/null
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_Benewake_TFMini.h
@@ -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; }
+};
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
index 0866e7689c..7d83aa549e 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder_MaxsonarSerialLV.h
@@ -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;
diff --git a/libraries/AP_RangeFinder/RangeFinder.cpp b/libraries/AP_RangeFinder/RangeFinder.cpp
index b9478a41ec..491dbb5d11 100644
--- a/libraries/AP_RangeFinder/RangeFinder.cpp
+++ b/libraries/AP_RangeFinder/RangeFinder.cpp
@@ -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: