zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

SITL: add support for simulated proximity sensors

zr-v5.1
Peter Barker 5 years ago committed by Peter Barker
parent
be9fc57e8f
commit
5bbb02e03b
5 changed files with 544 additions and 2 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 231
      libraries/SITL/SIM_PS_RPLidarA2.cpp
  2. 127
      libraries/SITL/SIM_PS_RPLidarA2.h
  3. 5
      libraries/SITL/SIM_SerialDevice.cpp
  4. 125
      libraries/SITL/SIM_SerialProximitySensor.cpp
  5. 58
      libraries/SITL/SIM_SerialProximitySensor.h

231
libraries/SITL/SIM_PS_RPLidarA2.cpp
Unescape View File

@ -0,0 +1,231 @@ @@ -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 <http://www.gnu.org/licenses/>.
*/
/*
Simulator for the RPLidarA2 proximity sensor
*/
#include "SIM_PS_RPLidarA2.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include <errno.h>
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<sample_count; i++) {
const float current_degrees_bf = fmod((last_degrees_bf + degrees_per_sample), 360.0f);
const uint8_t quality = 17; // random number
const uint16_t angle_q6 = current_degrees_bf * 64;
const bool is_start_packet = current_degrees_bf < last_degrees_bf;
last_degrees_bf = current_degrees_bf;
const float MAX_RANGE = 16.0f;
float distance = measure_distance_at_angle(location, current_degrees_bf);
// ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
if (distance > 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();
}
}

127
libraries/SITL/SIM_PS_RPLidarA2.h
Unescape View File

@ -0,0 +1,127 @@ @@ -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 <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 ./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 <stdio.h>
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;
};
};

5
libraries/SITL/SIM_SerialDevice.cpp
Unescape View File

@ -75,9 +75,10 @@ ssize_t SerialDevice::read_from_autopilot(char *buffer, const size_t size) @@ -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<ret; i++) {
// ::fprintf(stderr, "%02X", buffer[i]);
// const uint8_t x = buffer[i];
// ::fprintf(stderr, "%02X", (unsigned)x);
// }
// ::fprintf(stderr, " ");
// for (ssize_t i=0; i<ret; i++) {

125
libraries/SITL/SIM_SerialProximitySensor.cpp
Unescape View File

@ -0,0 +1,125 @@ @@ -0,0 +1,125 @@
/*
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/>.
*/
/*
Base class for serial proximity sensors
*/
#include "SIM_SerialProximitySensor.h"
#include <AP_Math/AP_Math.h>
#include <stdio.h>
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<num_post_offset; x++) {
for (int8_t y=-num_post_offset; y<num_post_offset; y++) {
Location post_location = post_origin;
post_location.offset(x*10+0.5, y*10+0.5);
if (postfile != nullptr) {
::fprintf(postfile, "map icon %f %f hoop\n", post_location.lat*1e-7, post_location.lng*1e-7);
}
Vector2f post_position_cm;
if (!post_location.get_vector_xy_from_origin_NE(post_position_cm)) {
// should probably use SITL variables...
return 0.0f;
}
const float radius_cm = 10.0f;
Vector2f intersection_point_cm;
if (Vector2f::circle_segment_intersection(pos1_cm, pos2_cm, post_position_cm, radius_cm, intersection_point_cm)) {
float dist_cm = (intersection_point_cm-pos1_cm).length();
if (intersectionsfile != nullptr) {
Location intersection_point = location;
intersection_point.offset(intersection_point_cm.x/100.0f,
intersection_point_cm.y/100.0f);
::fprintf(intersectionsfile,
"map icon %f %f barrell\n",
intersection_point.lat*1e-7,
intersection_point.lng*1e-7);
}
if (dist_cm < min_dist_cm) {
min_dist_cm = dist_cm;
}
}
}
}
if (postfile != nullptr) {
fclose(postfile);
}
if (intersectionsfile != nullptr) {
fclose(intersectionsfile);
}
// ::fprintf(stderr, "Distance @%f = %fm\n", angle, min_dist_cm/100.0f);
return min_dist_cm / 100.0f;
}

58
libraries/SITL/SIM_SerialProximitySensor.h
Unescape View File

@ -0,0 +1,58 @@ @@ -0,0 +1,58 @@
/*
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/>.
*/
/*
Base class for serial rangefinders
*/
#pragma once
#include "SIM_Aircraft.h"
#include <SITL/SITL.h>
#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
};
};
}
Write Preview
Loading…
Cancel
Save