ardupilot/libraries/SITL/SIM_PS_LightWare_SF45B.cpp

223 lines
7.1 KiB
C++

/*
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_LightWare_SF45B.h"
#include <GCS_MAVLink/GCS.h>
#include <stdio.h>
#include <errno.h>
using namespace SITL;
uint32_t PS_LightWare_SF45B::packet_for_location(const Location &location,
uint8_t *data,
uint8_t buflen)
{
return 0;
}
void PS_LightWare_SF45B::move_preamble_in_buffer(uint8_t search_start_pos)
{
uint8_t i;
for (i=search_start_pos; i<_buflen; i++) {
if ((uint8_t)_msg.buffer[i] == PREAMBLE) {
break;
}
}
if (i == 0) {
return;
}
memmove(_msg.buffer, &_msg.buffer[i], _buflen-i);
_buflen = _buflen - i;
}
// handle a valid message in _msg.buffer
void PS_LightWare_SF45B::send(const char *data, uint32_t len)
{
const ssize_t ret = write_to_autopilot(data, len);
if (ret < 0 || (uint32_t)ret != len) {
abort();
}
}
void PS_LightWare_SF45B::handle_message()
{
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Got message %u", (unsigned)_msg.packed_msgstream.msg.msgid);
switch (MessageID(_msg.packed_msgstream.msg.msgid)) {
case MessageID::STREAM: {
stream = _msg.packed_msgstream.msg.stream;
send_response.stream = true;
return;
}
case MessageID::DISTANCE_OUTPUT: {
desired_fields = _msg.packed_distance_output.msg.desired_fields;
send_response.desired_fields = true;
return;
}
case MessageID::UPDATE_RATE: {
update_rate = _msg.packed_update_rate.msg.rate;
send_response.update_rate = true;
return;
}
case MessageID::DISTANCE_DATA_CM: {
::fprintf(stderr, "Got a distance data. Weird.\n");
return;
}
}
// AP_HAL::panic("Unrecognised message (%u)", _msg.common.msgid);
::fprintf(stderr, "Unrecognised message (%u)\n", _msg.packed_msgstream.msg.msgid);
}
void PS_LightWare_SF45B::update_input()
{
const ssize_t n = read_from_autopilot(&_msg.buffer[_buflen], ARRAY_SIZE(_msg.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::WANT_PREAMBLE:
move_preamble_in_buffer();
if (_buflen == 0) {
return;
}
set_inputstate(InputState::WANT_FLAGS);
FALLTHROUGH;
case InputState::WANT_FLAGS:
if (_buflen < 4) { // 1 preamble, 2 flags, 1 msg
return;
}
set_inputstate(InputState::WANT_PAYLOAD);
FALLTHROUGH;
case InputState::WANT_PAYLOAD: {
// 1 preamble, 2 flags, 1 msg + payload
const uint8_t want_len = 4 + payload_length();
if (_buflen < want_len) {
return;
}
set_inputstate(InputState::WANT_CRC);
FALLTHROUGH;
}
case InputState::WANT_CRC: {
// 1 preamble, 2 flags, 1 msg + payload + 2 crc
const uint8_t want_len = 4 + payload_length() + 2;
if (_buflen < want_len) {
return;
}
const uint16_t got_checksum = UINT16_VALUE(uint8_t(_msg.buffer[want_len-1]), uint8_t(_msg.buffer[want_len-2]));
const uint16_t calc_checksum = msg_checksum();
if (calc_checksum == got_checksum) {
// GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-SF45B: Got one (%u)!", _msg.common.msgid);
// see if this was a read or a write request..
handle_message();
} else {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "sim-SF45B: Bad checksum");
}
// consume these bytes
move_preamble_in_buffer(want_len-1);
set_inputstate(InputState::WANT_PREAMBLE);
return;
}
}
}
void PS_LightWare_SF45B::update(const Location &location)
{
update_input();
update_output(location);
}
void PS_LightWare_SF45B::update_output(const Location &location)
{
switch (_state) {
case State::SCANNING:
update_output_responses();
update_output_scan(location);
return;
}
}
void PS_LightWare_SF45B::update_output_responses()
{
if (send_response.stream) {
send_response.stream = false;
PackedMessage<MsgStream> packed { MsgStream(stream), 0x1 };
packed.update_checksum();
send((char*)&packed, sizeof(packed));
}
if (send_response.update_rate) {
send_response.update_rate = false;
PackedMessage<UpdateRate> packed { UpdateRate(update_rate), 0x1 };
packed.update_checksum();
send((char*)&packed, sizeof(packed));
}
if (send_response.desired_fields) {
send_response.desired_fields = false;
PackedMessage<DistanceOutput> packed { DistanceOutput(desired_fields), 0x1 };
packed.update_checksum();
send((char*)&packed, sizeof(packed));
}
}
void PS_LightWare_SF45B::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 = 1000 / 1000.0f; // Randy reports 1 degree increments
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);
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) {
// sensor returns zero for out-of-range
distance = 0.0f;
}
PackedMessage<DistanceDataCM> packed_distance_data {
DistanceDataCM(
uint16_t(distance*100.0),
uint16_t(current_degrees_bf * 100)
), 0x1 };
packed_distance_data.update_checksum();
send((char*)&packed_distance_data, sizeof(packed_distance_data));
}
}