diff --git a/libraries/SITL/SIM_PS_LD06.cpp b/libraries/SITL/SIM_PS_LD06.cpp new file mode 100644 index 0000000000..416694a2e3 --- /dev/null +++ b/libraries/SITL/SIM_PS_LD06.cpp @@ -0,0 +1,162 @@ +/* + 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 LD06 proximity sensors +*/ + +#include "SIM_config.h" + +#if AP_SIM_PS_LD06_ENABLED + +#include "SIM_PS_LD06.h" + +#include +#include +#include +#include +#include + +using namespace SITL; + +uint32_t PS_LD06::packet_for_location(const Location &location, + uint8_t *data, + uint8_t buflen) +{ + return 0; +} + +void PS_LD06::update_input() +{ + // just discard any input + char buffer[256]; + uint8_t buflen = 0; + read_from_autopilot(&buffer[buflen], ARRAY_SIZE(buffer) - buflen - 1); +} + +void PS_LD06::update_output(const Location &location) +{ + const uint32_t now_ms = AP_HAL::millis(); + if (last_scan_output_time_ms == 0) { + last_scan_output_time_ms = now_ms; + return; + } + const uint32_t time_delta_ms = (now_ms - last_scan_output_time_ms); + + // the driver and device only send/handle 12 readings at a time at + // the moment + const uint32_t sample_count = 12; + + const uint32_t samples_per_second = 4500; + const float samples_per_ms = samples_per_second / 1000.0f; + + const uint32_t required_time_delta_ms = sample_count * samples_per_ms; + + if (time_delta_ms < required_time_delta_ms) { + return; + } + // sanity check we're being called often enough: + if (time_delta_ms / samples_per_ms > sample_count) { + AP_HAL::panic("Not being called often enough"); + } + + const float degrees_per_s = 2152; // see example datasheet page 8 + const float degrees_per_ms = degrees_per_s / 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_ms, degrees_per_sample); + + last_scan_output_time_ms += sample_count/samples_per_ms; + + const uint32_t required_send_buffer_size = 11 + 3*sample_count; + if (required_send_buffer_size > send_buffer_size) { + if (send_buffer != nullptr) { + free(send_buffer); + } + send_buffer = (uint8_t*)malloc(required_send_buffer_size); + if (send_buffer == nullptr) { + AP_BoardConfig::allocation_error("LD06 send buffer"); + } + send_buffer_size = required_send_buffer_size; + } + + struct PACKED PacketStart { + uint8_t preamble; + uint8_t length : 5; + uint8_t reserved : 3; + uint16_t angular_rate; // degrees/second + uint16_t start_angle; // centidegrees + }; + struct PACKED Measurement { + uint16_t distance; + uint8_t confidence; + }; + struct PACKED PacketEnd { + uint16_t end_angle; + uint16_t timestamp; + uint8_t crc; + }; + + PacketStart &packet_start = *((PacketStart*)send_buffer); + + packet_start.preamble = 0x54; + packet_start.length = sample_count; + packet_start.angular_rate = htole16(degrees_per_s); + packet_start.start_angle = htole16(last_degrees_bf * 100); + + uint16_t offset = sizeof(PacketStart); + + for (uint32_t i=0; i 12.0) { // 12 metre max range + // return 0 for out-of-range; is this correct? + distance = 0.0f; + confidence = 0; + } + + Measurement &measurement = *((Measurement*)&send_buffer[offset]); + + measurement.distance = htole16(distance * 1000); // m -> mm + measurement.confidence = confidence; + + offset += sizeof(Measurement); + } + + PacketEnd &packet_end = *((PacketEnd*)&send_buffer[offset]); + + packet_end.end_angle = htole16(last_degrees_bf * 100); // centidegrees + packet_end.timestamp = htole16(0); // milliseconds + offset += sizeof(PacketEnd); + + packet_end.crc = crc8_generic(&send_buffer[0], offset-1, 0x4D); + + const ssize_t ret = write_to_autopilot((const char*)send_buffer, offset); + if (ret != offset) { + // abort(); // there are startup issues with this, we fill things up before the driver is ready + } +} + +void PS_LD06::update(const Location &location) +{ + update_input(); + update_output(location); +} + +#endif // AP_SIM_PS_LD06_ENABLED diff --git a/libraries/SITL/SIM_PS_LD06.h b/libraries/SITL/SIM_PS_LD06.h new file mode 100644 index 0000000000..17f4995920 --- /dev/null +++ b/libraries/SITL/SIM_PS_LD06.h @@ -0,0 +1,86 @@ +/* + 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 LD06 proximity sensor + + Datasheet: https://storage.googleapis.com/mauser-public-images/prod_description_document/2021/315/8fcea7f5d479f4f4b71316d80b77ff45_096-6212_a.pdf + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:ld06 --speedup=1 -l 51.8752066,14.6487840,54.15,0 --map + +param set SERIAL5_PROTOCOL 11 +param set PRX1_TYPE 16 +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 +param set OA_TYPE 2 +reboot + +param ftp +param set OA_DB_OUTPUT 3 + +mode loiter +script /tmp/post-locations.scr +arm throttle +rc 3 1600 +rc 3 1500 +rc 2 1450 + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_PS_LD06_ENABLED + +#include "SIM_SerialProximitySensor.h" + +#include + +namespace SITL { + +class PS_LD06 : public SerialProximitySensor { +public: + + using SerialProximitySensor::SerialProximitySensor; + + 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(); // in the form of PWM (uninplemented) + void update_output(const Location &location); + + uint32_t last_scan_output_time_ms; + + float last_degrees_bf; + + uint8_t * send_buffer; + uint16_t send_buffer_size; +}; + +}; + +#endif // AP_SIM_PS_LD06_ENABLED diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 5d6850bd39..782b5b8b25 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -21,6 +21,10 @@ #define HAL_SIM_PS_RPLIDARA2_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED #endif +#ifndef AP_SIM_PS_LD06_ENABLED +#define AP_SIM_PS_LD06_ENABLED HAL_SIM_SERIALPROXIMITYSENSOR_ENABLED +#endif + #ifndef AP_SIM_IS31FL3195_ENABLED #define AP_SIM_IS31FL3195_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif