SITL: factor RPLidar simulation to allow for subclassing

libraries/SITL/SIM_PS_RPLidar.cpp

@@ -13,10 +13,10 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 /*
-  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,

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 <http://www.gnu.org/licenses/>.
+
+   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;
 };
 
 };

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  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; };
+};
+
+}