SITL: add LD06 simulator

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  Simulator for the LD06 proximity sensors
+*/
+
+#include "SIM_config.h"
+
+#if AP_SIM_PS_LD06_ENABLED
+
+#include "SIM_PS_LD06.h"
+
+#include <GCS_MAVLink/GCS.h>
+#include <AP_BoardConfig/AP_BoardConfig.h>
+#include <stdio.h>
+#include <errno.h>
+#include <AP_HAL/utility/sparse-endian.h>
+
+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<sample_count; i++) {
+        const float current_degrees_bf = fmod((last_degrees_bf + degrees_per_sample), 360.0f);
+        last_degrees_bf = current_degrees_bf;
+
+        float distance = measure_distance_at_angle_bf(location, current_degrees_bf);
+        // ::fprintf(stderr, "SIM: %f=%fm\n", current_degrees_bf, distance);
+
+        uint8_t confidence = 29; // random number; driver checks this
+        if (distance > 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

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 <http://www.gnu.org/licenses/>.
+
+  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 <stdio.h>
+
+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

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