From 5bbb02e03b1cd0cae79b3b30a64b808b844e0ddb Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 21 Nov 2019 13:12:23 +1100 Subject: [PATCH] SITL: add support for simulated proximity sensors --- libraries/SITL/SIM_PS_RPLidarA2.cpp | 231 +++++++++++++++++++ libraries/SITL/SIM_PS_RPLidarA2.h | 127 ++++++++++ libraries/SITL/SIM_SerialDevice.cpp | 5 +- libraries/SITL/SIM_SerialProximitySensor.cpp | 125 ++++++++++ libraries/SITL/SIM_SerialProximitySensor.h | 58 +++++ 5 files changed, 544 insertions(+), 2 deletions(-) create mode 100644 libraries/SITL/SIM_PS_RPLidarA2.cpp create mode 100644 libraries/SITL/SIM_PS_RPLidarA2.h create mode 100644 libraries/SITL/SIM_SerialProximitySensor.cpp create mode 100644 libraries/SITL/SIM_SerialProximitySensor.h diff --git a/libraries/SITL/SIM_PS_RPLidarA2.cpp b/libraries/SITL/SIM_PS_RPLidarA2.cpp new file mode 100644 index 0000000000..d304997c56 --- /dev/null +++ b/libraries/SITL/SIM_PS_RPLidarA2.cpp @@ -0,0 +1,231 @@ +/* + 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 +*/ + +#include "SIM_PS_RPLidarA2.h" + +#include +#include +#include + +using namespace SITL; + +uint32_t PS_RPLidarA2::packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) +{ + return 0; +} + +void PS_RPLidarA2::move_preamble_in_buffer() +{ + uint8_t i; + for (i=0; i<_buflen; i++) { + if ((uint8_t)_buffer[i] == PREAMBLE) { + break; + } + } + if (i == 0) { + return; + } + memmove(_buffer, &_buffer[i], _buflen-i); + _buflen = _buflen - i; +} + +void PS_RPLidarA2::update_input() +{ + const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1); + if (n < 0) { + // TODO: do better here + if (errno != EAGAIN && errno != EWOULDBLOCK && errno != 0) { + AP_HAL::panic("Failed to read from autopilot"); + } + } else { + _buflen += n; + } + + switch (_inputstate) { + case InputState::WAITING_FOR_PREAMBLE: + move_preamble_in_buffer(); + if (_buflen == 0) { + return; + } + set_inputstate(InputState::GOT_PREAMBLE); + // consume the preamble: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + FALLTHROUGH; + case InputState::GOT_PREAMBLE: + if (_buflen == 0) { + return; + } + switch ((Command)_buffer[0]) { + case Command::STOP: + // consume the command: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + set_state(State::IDLE); + return; + case Command::SCAN: + // consume the command: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + send_response_descriptor(0x05, SendMode::SRMR, DataType::Unknown81); + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + set_state(State::SCANNING); + return; + case Command::GET_HEALTH: { + // consume the command: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + send_response_descriptor(0x03, SendMode::SRSR, DataType::Unknown06); + // now send the health: + const uint8_t health[3] {}; // all zeros fine for now + const ssize_t ret = write_to_autopilot((const char*)health, ARRAY_SIZE(health)); + if (ret != ARRAY_SIZE(health)) { + abort(); + } + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + return; + } + case Command::FORCE_SCAN: + abort(); + case Command::RESET: + // consume the command: + memmove(_buffer, &_buffer[1], _buflen-1); + _buflen--; + set_inputstate(InputState::RESETTING_START); + return; + default: + AP_HAL::panic("Bad command received (%02x)", (uint8_t)_buffer[0]); + } + case InputState::RESETTING_START: + _firmware_info_offset = 0; + set_inputstate(InputState::RESETTING_SEND_FIRMWARE_INFO); + FALLTHROUGH; + case InputState::RESETTING_SEND_FIRMWARE_INFO: { + const ssize_t written = write_to_autopilot(&FIRMWARE_INFO[_firmware_info_offset], strlen(FIRMWARE_INFO) - _firmware_info_offset); + if (written <= 0) { + AP_HAL::panic("Failed to write to autopilot"); + } + _firmware_info_offset += written; + if (_firmware_info_offset < strlen(FIRMWARE_INFO)) { + return; + } + set_inputstate(InputState::WAITING_FOR_PREAMBLE); + return; + } + } +} + +void PS_RPLidarA2::update_output_scan(const Location &location) +{ + const uint32_t now = AP_HAL::millis(); + if (last_scan_output_time_ms == 0) { + last_scan_output_time_ms = now; + return; + } + const uint32_t time_delta = (now - last_scan_output_time_ms); + + const uint32_t samples_per_second = 1000; + const float samples_per_ms = samples_per_second / 1000.0f; + const uint32_t sample_count = time_delta / samples_per_ms; + const float degrees_per_ms = 3600 / 1000.0f; + const float degrees_per_sample = degrees_per_ms / samples_per_ms; + +// ::fprintf(stderr, "Packing %u samples in for %ums interval (%f degrees/sample)\n", sample_count, time_delta, degrees_per_sample); + + last_scan_output_time_ms += sample_count/samples_per_ms; + + for (uint32_t i=0; i MAX_RANGE) { + // sensor returns zero for out-of-range + distance = 0.0f; + } + const uint16_t distance_q2 = (distance*1000 * 4); // m->mm and *4 + + struct PACKED { + uint8_t startbit : 1; ///< on the first revolution 1 else 0 + uint8_t not_startbit : 1; ///< complementary to startbit + uint8_t quality : 6; ///< Related the reflected laser pulse strength + uint8_t checkbit : 1; ///< always set to 1 + uint16_t angle_q6 : 15; ///< Actual heading = angle_q6/64.0 Degree + uint16_t distance_q2 : 16; ///< Actual Distance = distance_q2/4.0 mm + } send_buffer; + send_buffer.startbit = is_start_packet; + send_buffer.not_startbit = !is_start_packet; + send_buffer.quality = quality; + send_buffer.checkbit = 1; + send_buffer.angle_q6 = angle_q6; + send_buffer.distance_q2 = distance_q2; + + static_assert(sizeof(send_buffer) == 5, "send_buffer correct size"); + + const ssize_t ret = write_to_autopilot((const char*)&send_buffer, sizeof(send_buffer)); + if (ret != sizeof(send_buffer)) { + abort(); + } + } +} + +void PS_RPLidarA2::update_output(const Location &location) +{ + switch (_state) { + case State::IDLE: + return; + case State::SCANNING: + update_output_scan(location); + return; + } +} + +void PS_RPLidarA2::update(const Location &location) +{ + update_input(); + update_output(location); +} + + +void PS_RPLidarA2::send_response_descriptor(uint32_t data_response_length, SendMode sendmode, DataType datatype) +{ + const uint8_t send_buffer[] = { + 0xA5, + 0x5A, + uint8_t((data_response_length >> 0) & 0xff), + uint8_t((data_response_length >> 8) & 0xff), + uint8_t((data_response_length >> 16) & 0xff), + uint8_t(((data_response_length >> 24) & 0xff) | (uint8_t) sendmode), + (uint8_t)datatype + }; + static_assert(ARRAY_SIZE(send_buffer) == 7, "send_buffer correct size"); + + const ssize_t ret = write_to_autopilot((const char*)send_buffer, ARRAY_SIZE(send_buffer)); + if (ret != ARRAY_SIZE(send_buffer)) { + abort(); + } +} diff --git a/libraries/SITL/SIM_PS_RPLidarA2.h b/libraries/SITL/SIM_PS_RPLidarA2.h new file mode 100644 index 0000000000..1cdf622732 --- /dev/null +++ b/libraries/SITL/SIM_PS_RPLidarA2.h @@ -0,0 +1,127 @@ +/* + 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 ./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:rplidara2 --speedup=1 -l 51.8752066,14.6487840,0,0 + +param set SERIAL5_PROTOCOL 11 +param set PRX_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 +reboot +mode loiter +script /tmp/post-locations.scr +arm throttle +rc 3 1600 +rc 3 1500 +rc 2 1450 + +*/ + +#pragma once + +#include "SIM_SerialProximitySensor.h" + +#include + +namespace SITL { + +class PS_RPLidarA2 : public SerialProximitySensor { +public: + + uint32_t packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) override; + + void update(const Location &location) override; + +private: + + void update_input(); + void update_output(const Location &location); + void update_output_scan(const Location &location); + + uint32_t last_scan_output_time_ms; + + float last_degrees_bf; + + char _buffer[256]; // from-autopilot + uint8_t _buflen; + + // TODO: see what happens on real device on partial input + enum class State { + IDLE = 17, + SCANNING = 18, + }; + State _state = State::IDLE; + void set_state(State newstate) { + ::fprintf(stderr, "Moving from state (%u) to (%u)\n", (uint8_t)_state, (uint8_t)newstate); + _state = newstate; + } + + enum class InputState { + WAITING_FOR_PREAMBLE = 45, + GOT_PREAMBLE = 46, + RESETTING_START = 47, + RESETTING_SEND_FIRMWARE_INFO = 48, + }; + InputState _inputstate = InputState::WAITING_FOR_PREAMBLE; + void set_inputstate(InputState newstate) { + ::fprintf(stderr, "Moving from inputstate (%u) to (%u)\n", (uint8_t)_state, (uint8_t)newstate); + _inputstate = newstate; + } + + static const uint8_t PREAMBLE = 0xA5; + + enum class Command { + STOP = 0x25, + SCAN = 0x20, + FORCE_SCAN = 0x21, + RESET = 0x40, + GET_HEALTH = 0x52, + }; + + void move_preamble_in_buffer(); + + enum class DataType { + Unknown06 = 0x06, // uint8_t ?! + Unknown81 = 0x81, // uint8_t ?! + }; + + enum class SendMode { + SRSR = 0x00, + SRMR = 0x40, // no idea why this isn't 0x01 + }; + + void send_response_descriptor(uint32_t data_response_length, + SendMode sendmode, + DataType datatype); + + + // the driver expects to see an "R" followed by 62 bytes more crap. + static const constexpr char *FIRMWARE_INFO = "R12345678901234567890123456789012345678901234567890123456789012"; + uint8_t _firmware_info_offset; +}; + +}; diff --git a/libraries/SITL/SIM_SerialDevice.cpp b/libraries/SITL/SIM_SerialDevice.cpp index 6ba0f0607f..7da982c37a 100644 --- a/libraries/SITL/SIM_SerialDevice.cpp +++ b/libraries/SITL/SIM_SerialDevice.cpp @@ -75,9 +75,10 @@ ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) { const ssize_t ret = ::read(read_fd_my_end, buffer, size); // if (ret > 0) { - // ::fprintf(stderr, "SIM_SerialDevice: read from autopilot: ("); + // ::fprintf(stderr, "SIM_SerialDevice: read from autopilot (%u): (", (unsigned)ret); // for (ssize_t i=0; i. + */ +/* + Base class for serial proximity sensors +*/ + +#include "SIM_SerialProximitySensor.h" + +#include + +#include + +using namespace SITL; + +void SerialProximitySensor::update(const Location &location) +{ + // just send a chunk of data at 5Hz: + const uint32_t now = AP_HAL::millis(); + if (now - last_sent_ms < reading_interval_ms()) { + return; + } + last_sent_ms = now; + + uint8_t data[255]; + const uint32_t packetlen = packet_for_location(location, + data, + ARRAY_SIZE(data)); + + write_to_autopilot((char*)data, packetlen); +} + +float SerialProximitySensor::measure_distance_at_angle(const Location &location, float angle) const +{ + // const SITL *sitl = AP::sitl(); + + Vector2f pos1_cm; + if (!location.get_vector_xy_from_origin_NE(pos1_cm)) { + // should probably use SITL variables... + return 0.0f; + } + static uint64_t count = 0; + count++; + + // the 1000 here is so the files don't grow unbounded + const bool write_debug_files = true && count < 1000; + + FILE *rayfile = nullptr; + if (write_debug_files) { + rayfile = fopen("/tmp/rayfile.scr", "a"); + } + // cast a ray from location out 200m... + Location location2 = location; + location2.offset_bearing(wrap_180(angle), 200); + Vector2f pos2_cm; + if (!location2.get_vector_xy_from_origin_NE(pos2_cm)) { + // should probably use SITL variables... + return 0.0f; + } + if (rayfile != nullptr) { + ::fprintf(rayfile, "map icon %f %f barrell\n", location2.lat*1e-7, location2.lng*1e-7); + fclose(rayfile); + } + + // setup a grid of posts + FILE *postfile = nullptr; + FILE *intersectionsfile = nullptr; + if (write_debug_files) { + postfile = fopen("/tmp/post-locations.scr", "w"); + intersectionsfile = fopen("/tmp/intersections.scr", "a"); + } + float min_dist_cm = 1000000.0; + const uint8_t num_post_offset = 10; + for (int8_t x=-num_post_offset; x. + */ +/* + Base class for serial rangefinders +*/ + +#pragma once + +#include "SIM_Aircraft.h" + +#include + +#include "SIM_SerialDevice.h" + +namespace SITL { + +class SerialProximitySensor : public SerialDevice { +public: + + SerialProximitySensor() {}; + + // update state + virtual void update(const Location &location); + + virtual uint16_t reading_interval_ms() const { return 200; } // 5Hz default + + virtual uint32_t packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) = 0; + + // return distance to nearest object at angle + float measure_distance_at_angle(const Location &location, float angle) const; + +private: + + uint32_t last_sent_ms; + + const Location post_origin { + 518752066, + 146487830, + 0, + Location::AltFrame::ABSOLUTE + }; +}; + +}