2020-12-08 06:13:41 -04:00
|
|
|
/*
|
|
|
|
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"
|
|
|
|
|
2021-10-11 01:59:19 -03:00
|
|
|
#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
|
|
|
|
|
2020-12-08 06:13:41 -04:00
|
|
|
#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;
|
2021-02-04 00:48:04 -04:00
|
|
|
const float degrees_per_ms = 1000 / 1000.0f; // Randy reports 1 degree increments
|
2020-12-08 06:13:41 -04:00
|
|
|
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));
|
|
|
|
}
|
|
|
|
}
|
2021-10-11 01:59:19 -03:00
|
|
|
|
|
|
|
#endif // HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
|