LD06: Record only 2deg slices

Tools/autotest/arducopter.py

@@ -6158,7 +6158,7 @@
 
             # double-notch should do better, but check for within 5%
             if peakdb2 * 1.05 > peakdb1:
                 raise NotAchievedException(
                     "Double-notch peak was higher than single-notch peak %fdB > %fdB" %
                     (peakdb2, peakdb1))
 
@@ -6171,7 +6171,7 @@
 
             # triple-notch should do better, but check for within 5%
             if peakdb2 * 1.05 > peakdb1:
                 raise NotAchievedException(
                     "Triple-notch peak was higher than single-notch peak %fdB > %fdB" %
                     (peakdb2, peakdb1))
 
@@ -6182,7 +6182,7 @@
         self.context_pop()
 
         if ex is not None:
             raise ex
 
     def DynamicRpmNotches(self):
         """Use dynamic harmonic notch to control motor noise via ESC telemetry."""
@@ -7582,6 +7582,16 @@
             "OA_TYPE": 2,
         })
         sensors = [  # tuples of name, prx_type
+            ('ld06', 16, {
+                mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 273,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 256,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_90: 1130,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_135: 696,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_180: 625,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_225: 967,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_270: 760,
+                mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_315: 771,
+            }),
             ('sf45b', 8, {
                 mavutil.mavlink.MAV_SENSOR_ROTATION_NONE: 270,
                 mavutil.mavlink.MAV_SENSOR_ROTATION_YAW_45: 258,

libraries/AP_HAL_SITL/SITL_State_common.cpp

@@ -184,6 +184,14 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
         crsf = NEW_NOTHROW SITL::CRSF();
         return crsf;
 #endif
+#if AP_SIM_PS_LD06_ENABLED
+    } else if (streq(name, "ld06")) {
+        if (ld06 != nullptr) {
+            AP_HAL::panic("Only one ld06 at a time");
+        }
+        ld06 = NEW_NOTHROW SITL::PS_LD06();
+        return ld06;
+#endif  // AP_SIM_PS_LD06_ENABLED
 #if HAL_SIM_PS_RPLIDARA2_ENABLED
     } else if (streq(name, "rplidara2")) {
         if (rplidara2 != nullptr) {
@@ -431,6 +439,12 @@ void SITL_State_Common::sim_update(void)
     }
 #endif
 
+#if AP_SIM_PS_LD06_ENABLED
+    if (ld06 != nullptr) {
+        ld06->update(sitl_model->get_location());
+    }
+#endif  // AP_SIM_PS_LD06_ENABLED
+
 #if HAL_SIM_PS_RPLIDARA2_ENABLED
     if (rplidara2 != nullptr) {
         rplidara2->update(sitl_model->get_location());

libraries/AP_HAL_SITL/SITL_State_common.h

@@ -44,6 +44,7 @@
 #include <SITL/SIM_CRSF.h>
 // #include <SITL/SIM_Frsky_SPort.h>
 // #include <SITL/SIM_Frsky_SPortPassthrough.h>
+#include <SITL/SIM_PS_LD06.h>
 #include <SITL/SIM_PS_RPLidarA2.h>
 #include <SITL/SIM_PS_RPLidarA1.h>
 #include <SITL/SIM_PS_TeraRangerTower.h>
@@ -172,6 +173,11 @@ class HALSITL::SITL_State_Common {
     // SITL::Frsky_SPort *frsky_sport;
     // SITL::Frsky_SPortPassthrough *frsky_sportpassthrough;
 
+#if AP_SIM_PS_LD06_ENABLED
+    // simulated LD06:
+    SITL::PS_LD06 *ld06;
+#endif  // AP_SIM_PS_LD06_ENABLED
+
 #if HAL_SIM_PS_RPLIDARA2_ENABLED
     // simulated RPLidarA2:
     SITL::PS_RPLidarA2 *rplidara2;

libraries/AP_Proximity/AP_Proximity_LD06.cpp

@@ -106,8 +106,10 @@ void AP_Proximity_LD06::get_readings()
                 continue;
             }
 
-            const uint32_t total_packet_length = _response[START_DATA_LENGTH] + 3;
-            if (total_packet_length > ARRAY_SIZE(_response)) {
+            // total_packet_length = sizeof(header) + datalength + sizeof(footer):
+            const uint32_t total_packet_length = 6 + 3*_response[START_DATA_LENGTH] + 5;
+            if (_response[START_DATA_LENGTH] != PAYLOAD_COUNT ||
+                total_packet_length > ARRAY_SIZE(_response)) {
                 // invalid packet received; throw away all data and
                 // start again.
                 _byte_count = 0;
@@ -121,15 +123,10 @@ void AP_Proximity_LD06::get_readings()
 
                 const uint32_t current_ms = AP_HAL::millis();
 
-                // Stores the last distance taken, used to reduce number of readings taken
-                if (_last_distance_received_ms != current_ms) {
-                    _last_distance_received_ms =  current_ms;
-                }
+                _last_distance_received_ms =  current_ms;
 
                 // Updates the temporary boundary and passes off the completed data
                 parse_response_data();
-                _temp_boundary.update_3D_boundary(state.instance, frontend.boundary);
-                _temp_boundary.reset();
 
                 // Resets the bytes read and whether or not we are reading data to accept a new payload
                 _byte_count = 0;
@@ -148,62 +145,75 @@ void AP_Proximity_LD06::parse_response_data()
     // Second byte in array stores length of data - not used but stored for debugging
     // const uint8_t data_length = _response[START_DATA_LENGTH];
 
-    // Respective bits store the radar speed, start/end angles
-    // Use bitwise operations to correctly obtain correct angles
-    // Divide angles by 100 as per manual
-    const float start_angle = float(UINT16_VALUE(_response[START_BEGIN_ANGLE + 1], _response[START_BEGIN_ANGLE])) * 0.01;
-    const float end_angle = float(UINT16_VALUE(_response[START_END_ANGLE + 1], _response[START_END_ANGLE])) * 0.01;
-
     // Verify the checksum that is stored in the last element of the response array
     // Return if checksum is incorrect - i.e. bad data, bad readings, etc.
     const uint8_t check_sum = _response[START_CHECK_SUM];
     if (check_sum != crc8_generic(&_response[0], sizeof(_response) / sizeof(_response[0]) - 1, 0x4D)) {
         return;
     }
 
-    // Calculates the angle that this point was sampled at
-    float sampled_counts = 0;
-    const float angle_step = (end_angle - start_angle) /  (PAYLOAD_COUNT - 1);
-    float uncorrected_angle = start_angle + (end_angle - start_angle) * 0.5;
+    // Respective bits store the radar speed, start/end angles
+    // Use bitwise operations to correctly obtain correct angles
+    // Divide angles by 100 as per manual
+    const float start_angle = float(UINT16_VALUE(_response[START_BEGIN_ANGLE + 1], _response[START_BEGIN_ANGLE])) * 0.01;
+    const float end_angle = float(UINT16_VALUE(_response[START_END_ANGLE + 1], _response[START_END_ANGLE])) * 0.01;
 
-    // Handles the case that the angles read went from 360 to 0 (jumped)
-    if (angle_step < 0) {
-        uncorrected_angle = wrap_360(start_angle + (end_angle + 360 - start_angle) * 0.5);
+    float angle_step;
+    if (start_angle < end_angle) {
+        angle_step = (end_angle - start_angle) / (PAYLOAD_COUNT - 1);
+    } else {
+        angle_step = (end_angle + 360 - start_angle) / (PAYLOAD_COUNT - 1);
     }
-
-    // Takes the angle in the middle of the readings to be pushed to the database
-    const float push_angle = correct_angle_for_orientation(uncorrected_angle);
-
-    float distance_avg = 0.0;
-
     // Each recording point is three bytes long, goes through all of that and updates database
     for (uint16_t i = START_PAYLOAD; i < START_PAYLOAD + MEASUREMENT_PAYLOAD_LENGTH * PAYLOAD_COUNT; i += MEASUREMENT_PAYLOAD_LENGTH) {
 
         // Gets the distance recorded and converts to meters
-        const float distance_meas = UINT16_VALUE(_response[i + 1], _response[i]) * 0.001;
+        const float angle_deg = correct_angle_for_orientation(start_angle + angle_step * (i / MEASUREMENT_PAYLOAD_LENGTH));
+        const float distance_m = UINT16_VALUE(_response[i + 1], _response[i]) * 0.001;
 
-        // Validates data and checks if it should be included
-        if (distance_meas > distance_min() && distance_meas < distance_max()) {
-            if (ignore_reading(push_angle, distance_meas)) {
-                continue;
+        if (distance_m < distance_min() || distance_m > distance_max() || _response[i + 2] < 20) { // XXX 20 good?
+            continue;
+        }
+        if (ignore_reading(angle_deg, distance_m)) {
+            continue;
+        }
+
+        uint16_t a2d = (int)(angle_deg / 2.0) * 2;
+        if (_angle_2deg == a2d) {
+            if (distance_m < _dist_2deg_m) {
+                _dist_2deg_m = distance_m;
             }
+        } else {
+            // New 2deg angle, record the old one
+
+            const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face((float)_angle_2deg);
+
+            if (face != _last_face) {
+                // distance is for a new face, the previous one can be updated now
+                if (_last_distance_valid) {
+                    frontend.boundary.set_face_attributes(_last_face, _last_angle_deg, _last_distance_m, state.instance);
+                } else {
+                    // reset distance from last face
+                    frontend.boundary.reset_face(face, state.instance);
+                }
 
-            sampled_counts ++;
-            distance_avg += distance_meas;
-        }
-    }
+                // initialize the new face
+                _last_face = face;
+                _last_distance_valid = false;
+            }
 
-    // Convert angle to appropriate face and adds to database
-    // Since angle increments are only about 3 degrees, ignore readings if there were only 1 or 2 measurements
-    //    (likely outliers) recorded in the range
-    if (sampled_counts > 2) {
-        // Gets the average distance read
-        distance_avg /= sampled_counts;
-
-        // Pushes the average distance and angle to the obstacle avoidance database
-        const AP_Proximity_Boundary_3D::Face face = frontend.boundary.get_face(push_angle);
-        _temp_boundary.add_distance(face, push_angle, distance_avg);
-        database_push(push_angle, distance_avg);
+            // update shortest distance
+            if (!_last_distance_valid || (_dist_2deg_m < _last_distance_m)) {
+                _last_distance_m = _dist_2deg_m;
+                _last_distance_valid = true;
+                _last_angle_deg = (float)_angle_2deg;
+            }
+            // update OA database
+            database_push(_last_angle_deg, _last_distance_m);
+
+            _angle_2deg = a2d;
+            _dist_2deg_m = distance_m;
+        }
     }
 }
 #endif // AP_PROXIMITY_LD06_ENABLED

libraries/AP_Proximity/AP_Proximity_LD06.h

@@ -62,7 +62,13 @@ class AP_Proximity_LD06 : public AP_Proximity_Backend_Serial
     // Store for error-tracking purposes
     uint32_t  _last_distance_received_ms;
 
-    // Boundary to store the measurements
-    AP_Proximity_Temp_Boundary _temp_boundary;
+    // face related variables
+    AP_Proximity_Boundary_3D::Face _last_face;///< last face requested
+    float _last_angle_deg;                    ///< yaw angle (in degrees) of _last_distance_m
+    float _last_distance_m;                   ///< shortest distance for _last_face
+    bool _last_distance_valid;                ///< true if _last_distance_m is valid
+
+    uint16_t _angle_2deg;
+    float _dist_2deg_m;
 };
 #endif // AP_PROXIMITY_LD06_ENABLED

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