aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: add support for simulated proximity sensors

Browse Source
This commit is contained in:
Peter Barker 2019-11-21 13:12:23 +11:00 committed by Peter Barker
parent be9fc57e8f
commit 5bbb02e03b
5 changed files with 544 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

231
libraries/SITL/SIM_PS_RPLidarA2.cpp Normal file
View File

@ -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 Normal file
View File

@ -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
View File

@ -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 Normal file
View File

@ -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 Normal file
View File

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