Added Ld19 Sensor Driver Interface

libraries/AP_Proximity/AP_Proximity.cpp

@@ -30,6 +30,7 @@
 #include "AP_Proximity_DroneCAN.h"
 #include "AP_Proximity_Scripting.h"
 #include "AP_Proximity_LD06.h"
+#include "AP_Proximity_LD19.h"
 
 #include <AP_Logger/AP_Logger.h>
 
@@ -177,6 +178,17 @@ void AP_Proximity::init()
             }
             break;
 #endif
+
+#if AP_PROXIMITY_LD19_ENABLED
+        case Type::LD19:
+            if (AP_Proximity_LD19::detect(serial_instance)) {
+                state[instance].instance = instance;
+                drivers[instance] = new AP_Proximity_LD19(*this, state[instance], params[instance], serial_instance);
+                serial_instance++;
+            }
+            break;
+#endif
+
 #if AP_PROXIMITY_LIGHTWARE_SF45B_ENABLED
         case Type::SF45B:
             if (AP_Proximity_LightWareSF45B::detect(serial_instance)) {

libraries/AP_Proximity/AP_Proximity.h

@@ -86,6 +86,9 @@ class AP_Proximity
 #endif
 #if AP_PROXIMITY_LD06_ENABLED
         LD06 = 16,
+#endif
+#if AP_PROXIMITY_LD19_ENABLED
+        LD19 = 17,
 #endif
     };
 

libraries/AP_Proximity/AP_Proximity_LD19.cpp

@@ -0,0 +1,174 @@
+
+#include "AP_Proximity_config.h"
+
+#if AP_PROXIMITY_LD19_ENABLED
+#include "AP_Proximity_LD19.h"
+
+#define LD_START_CHAR       0x54
+#define PROXIMITY_LD19_TIMEOUT_MS 50
+
+// Indices in data array where each value starts being recorded
+// See comment below about data payload for more info about formatting
+#define START_BEGIN_CHARACTER       0
+#define START_DATA_LENGTH           1
+#define START_RADAR_SPEED           2
+#define START_BEGIN_ANGLE           4
+#define START_PAYLOAD               6
+#define START_END_ANGLE             42
+#define START_CHECK_SUM             46
+#define MEASUREMENT_PAYLOAD_LENGTH  3
+#define PAYLOAD_COUNT               12
+
+ /* ------------------------------------------
+    Data Packet Structure:
+    Start Character : 1 Byte
+    Data Length : 1 Byte
+    Radar Speed : 2 Bytes
+    Start Angle : 2 Bytes
+    Data Measurements : 36 Bytes
+        Contains 12 measurements of 3 Bytes each
+        Each measurement has 2 Bytes for distance to closest object
+        Each measurement has the 3rd Byte as measurement Confidence
+    End Angle : 2 Bytes
+    Timestamp : 2 Bytes
+    Checksum : 1 Byte
+    ------------------------------------------ */
+// ----> 47 data bytes in total for one packet
+
+// Update the sensor readings
+void AP_Proximity_LD19::update(void)
+{
+    // Escape if no connection detected/supported while running
+    if (_uart == nullptr) {
+        return;
+    }
+
+    // Begin getting sensor readings
+    // Calls method that repeatedly reads through UART channel
+    get_readings();
+
+    // Check if the data is being received correctly and sets Proximity Status
+    if (_last_distance_received_ms == 0 || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_LD19_TIMEOUT_MS)) {
+        set_status(AP_Proximity::Status::NoData);
+    } else {
+        set_status(AP_Proximity::Status::Good);
+    }
+}
+
+// Called repeatedly to get the readings at the current instant
+void AP_Proximity_LD19::get_readings()
+{
+    if (_uart == nullptr) {
+        return;
+    }
+
+    // Store the number of bytes available on the UART input
+    uint32_t nbytes = MIN((uint16_t) 4000,  _uart->available());
+
+    // Loops through all bytes that were received
+    while (nbytes-- > 0) {
+
+        // Gets and logs the current byte being read
+        const uint8_t c = _uart->read();
+
+        // Stores the byte in an array if the byte is a start byte or we have already read a start byte
+        if (c == LD_START_CHAR || _response_data) {
+
+            // Sets to true if a start byte has been read, default false otherwise
+            _response_data = true;
+
+            // Stores the next byte in an array
+            _response[_byte_count] = c;
+            _byte_count++;
+
+            if (_byte_count == _response[START_DATA_LENGTH] + 3) {
+
+                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;
+                }
+
+                // 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;
+                _response_data = false;
+            }
+        }
+    }
+}
+
+// Parses the data packet received from the LiDAR
+void AP_Proximity_LD19::parse_response_data()
+{
+
+    // Data interpretation based on:
+    // http://wiki.inno-maker.com/display/HOMEPAGE/LD19?preview=/6949506/6949511/LDROBOT_LD19_Development%20manual_v1.0_en.pdf
+
+    // 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;
+
+    // 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);
+    }
+
+    // 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;
+
+        // 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;
+            }
+
+            sampled_counts ++;
+            distance_avg += distance_meas;
+        }
+    }
+
+    // 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);
+    }
+}
+#endif // AP_PROXIMITY_LD19_ENABLED

libraries/AP_Proximity/AP_Proximity_LD19.h

@@ -0,0 +1,44 @@
+
+#pragma once
+#include "AP_Proximity_config.h"
+
+#if AP_PROXIMITY_LD19_ENABLED
+#include "AP_Proximity_Backend_Serial.h"
+
+#define MESSAGE_LENGTH_LD19         47
+
+// Minimum and maximum distance that the sensor can read in meters
+#define MAX_READ_DISTANCE_LD19          12.0f
+#define MIN_READ_DISTANCE_LD19           0.02f
+
+class AP_Proximity_LD19 : public AP_Proximity_Backend_Serial
+{
+public:
+
+    using AP_Proximity_Backend_Serial::AP_Proximity_Backend_Serial;
+
+    // Update the state of the sensor
+    void update(void) override;
+
+    // Get the max and min distances for the sensor being used
+    float distance_max() const override { return MAX_READ_DISTANCE_LD19; }
+    float distance_min() const override { return MIN_READ_DISTANCE_LD19; }
+
+private:
+
+    // Get and parse the sensor data
+    void parse_response_data();
+    void get_readings();
+
+    // Store and keep track of the bytes being read from the sensor
+    uint8_t _response[MESSAGE_LENGTH_LD19];
+    bool _response_data;
+    uint16_t _byte_count;
+
+    // Store for error-tracking purposes
+    uint32_t  _last_distance_received_ms;
+
+    // Boundary to store the measurements
+    AP_Proximity_Temp_Boundary _temp_boundary;
+};
+#endif // AP_PROXIMITY_LD19_ENABLED

libraries/AP_Proximity/AP_Proximity_config.h

@@ -68,3 +68,7 @@
 #ifndef AP_PROXIMITY_LD06_ENABLED
 #define AP_PROXIMITY_LD06_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
 #endif
+
+#ifndef AP_PROXIMITY_LD19_ENABLED
+#define AP_PROXIMITY_LD19_ENABLED AP_PROXIMITY_BACKEND_DEFAULT_ENABLED
+#endif