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; }; +}; + +}