diff --git a/libraries/SITL/SIM_PS_RPLidar.cpp b/libraries/SITL/SIM_PS_RPLidar.cpp
index 1da08f4262..99638aa034 100644
--- a/libraries/SITL/SIM_PS_RPLidar.cpp
+++ b/libraries/SITL/SIM_PS_RPLidar.cpp
@@ -13,10 +13,10 @@
along with this program. If not, see .
*/
/*
- Simulator for the RPLidarA2 proximity sensor
+ Simulator for the RPLidar proximity sensors
*/
-#include "SIM_PS_RPLidarA2.h"
+#include "SIM_PS_RPLidar.h"
#if HAL_SIM_PS_RPLIDARA2_ENABLED
@@ -26,14 +26,14 @@
using namespace SITL;
-uint32_t PS_RPLidarA2::packet_for_location(const Location &location,
+uint32_t PS_RPLidar::packet_for_location(const Location &location,
uint8_t *data,
uint8_t buflen)
{
return 0;
}
-void PS_RPLidarA2::move_preamble_in_buffer()
+void PS_RPLidar::move_preamble_in_buffer()
{
uint8_t i;
for (i=0; i<_buflen; i++) {
@@ -48,7 +48,7 @@ void PS_RPLidarA2::move_preamble_in_buffer()
_buflen = _buflen - i;
}
-void PS_RPLidarA2::update_input()
+void PS_RPLidar::update_input()
{
const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
if (n < 0) {
@@ -159,7 +159,7 @@ void PS_RPLidarA2::update_input()
}
}
-void PS_RPLidarA2::update_output_scan(const Location &location)
+void PS_RPLidar::update_output_scan(const Location &location)
{
const uint32_t now = AP_HAL::millis();
if (last_scan_output_time_ms == 0) {
@@ -186,10 +186,9 @@ void PS_RPLidarA2::update_output_scan(const Location &location)
last_degrees_bf = current_degrees_bf;
- const float MAX_RANGE = 16.0f;
float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
// ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
- if (distance > MAX_RANGE) {
+ if (distance > max_range()) {
// sensor returns zero for out-of-range
distance = 0.0f;
}
@@ -219,7 +218,7 @@ void PS_RPLidarA2::update_output_scan(const Location &location)
}
}
-void PS_RPLidarA2::update_output(const Location &location)
+void PS_RPLidar::update_output(const Location &location)
{
switch (_state) {
case State::IDLE:
@@ -230,14 +229,14 @@ void PS_RPLidarA2::update_output(const Location &location)
}
}
-void PS_RPLidarA2::update(const Location &location)
+void PS_RPLidar::update(const Location &location)
{
update_input();
update_output(location);
}
-void PS_RPLidarA2::send_response_descriptor(uint32_t data_response_length, SendMode sendmode, DataType datatype)
+void PS_RPLidar::send_response_descriptor(uint32_t data_response_length, SendMode sendmode, DataType datatype)
{
const uint8_t send_buffer[] = {
0xA5,
diff --git a/libraries/SITL/SIM_PS_RPLidar.h b/libraries/SITL/SIM_PS_RPLidar.h
index cb8a4a4233..815ed6c9c2 100644
--- a/libraries/SITL/SIM_PS_RPLidar.h
+++ b/libraries/SITL/SIM_PS_RPLidar.h
@@ -11,6 +11,8 @@
You should have received a copy of the GNU General Public License
along with this program. If not, see .
+
+ Base class for RPLidar support
*/
/*
Simulator for the RPLidarA2 proximity sensor
@@ -55,7 +57,7 @@ rc 2 1450
namespace SITL {
-class PS_RPLidarA2 : public SerialProximitySensor {
+class PS_RPLidar : public SerialProximitySensor {
public:
using SerialProximitySensor::SerialProximitySensor;
@@ -135,8 +137,9 @@ private:
static const constexpr char *FIRMWARE_INFO = "R12345678901234567890123456789012345678901234567890123456789012";
uint8_t _firmware_info_offset;
- // this will be pure-virtual in a notional RPLidar base class:
- uint8_t device_info_model() const { return 0x28; }
+ // methods for sub-classes to implement:
+ virtual uint8_t device_info_model() const = 0;
+ virtual uint8_t max_range() const = 0;
};
};
diff --git a/libraries/SITL/SIM_PS_RPLidarA2.h b/libraries/SITL/SIM_PS_RPLidarA2.h
new file mode 100644
index 0000000000..dfef555664
--- /dev/null
+++ b/libraries/SITL/SIM_PS_RPLidarA2.h
@@ -0,0 +1,56 @@
+/*
+ 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 .
+ */
+/*
+ Simulator for the RPLidarA2 proximity sensor
+
+./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,0,0 --map
+
+param set SERIAL5_PROTOCOL 11
+param set PRX1_TYPE 5
+reboot
+
+arm throttle
+rc 3 1600
+
+# for avoidance:
+param set DISARM_DELAY 0
+param set AVOID_ENABLE 2 # use proximity sensor
+param set AVOID_MARGIN 2.00 # 2m
+param set AVOID_BEHAVE 0 # slide
+param set OA_DB_OUTPUT 3
+param set OA_TYPE 2
+reboot
+mode loiter
+script /tmp/post-locations.scr
+arm throttle
+rc 3 1600
+rc 3 1500
+rc 2 1450
+
+*/
+
+#pragma once
+
+#include "SIM_PS_RPLidar.h"
+
+namespace SITL {
+
+class PS_RPLidarA2 : public PS_RPLidar {
+public:
+ uint8_t device_info_model() const override { return 0x28; }
+ uint8_t max_range() const override { return 16.0f; };
+};
+
+}