AP_RangeFinder: LeddarVu8 driver

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -42,6 +42,7 @@
 #include "AP_RangeFinder_BLPing.h"
 #include "AP_RangeFinder_UAVCAN.h"
 #include "AP_RangeFinder_Lanbao.h"
+#include "AP_RangeFinder_LeddarVu8.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -496,6 +497,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
             drivers[instance] = new AP_RangeFinder_Lanbao(state[instance], params[instance], serial_instance++);
         }
         break;
+    case Type::LeddarVu8_Serial:
+        if (AP_RangeFinder_Lanbao::detect(serial_instance)) {
+            drivers[instance] = new AP_RangeFinder_LeddarVu8(state[instance], params[instance], serial_instance++);
+        }
+        break;
     default:
         break;
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -78,6 +78,7 @@ public:
         Lanbao = 26,
         BenewakeTF03 = 27,
         VL53L1X_Short = 28,
+        LeddarVu8_Serial = 29,
     };
 
     enum class Function {

libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.cpp

@@ -0,0 +1,204 @@
+/*
+   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/>.
+ */
+
+#include "AP_RangeFinder_LeddarVu8.h"
+
+#include <AP_HAL/AP_HAL.h>
+#include <ctype.h>
+
+extern const AP_HAL::HAL& hal;
+
+// LeddarVu8 uses the modbus RTU protocol
+// https://autonomoustuff.com/product/leddartech-vu8/
+
+#define LEDDARVU8_ADDR_DEFAULT              0x01        // modbus default device id
+#define LEDDARVU8_DIST_MAX_CM               18500       // maximum possible distance reported by lidar
+#define LEDDARVU8_OUT_OF_RANGE_ADD_CM       100         // add this many cm to out-of-range values
+#define LEDDARVU8_TIMEOUT_MS                200         // timeout in milliseconds if no distance messages received
+
+// distance returned in reading_cm, signal_ok is set to true if sensor reports a strong signal
+bool AP_RangeFinder_LeddarVu8::get_reading(uint16_t &reading_cm)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // check for timeout receiving messages
+    uint32_t now_ms = AP_HAL::millis();
+    if (((now_ms - last_distance_ms) > LEDDARVU8_TIMEOUT_MS) && ((now_ms - last_distance_request_ms) > LEDDARVU8_TIMEOUT_MS)) {
+        request_distances();
+    }
+
+    // variables for averaging results from multiple messages
+    float sum_cm = 0;
+    uint16_t count = 0;
+    uint16_t count_out_of_range = 0;
+    uint16_t latest_dist_cm = 0;
+    bool latest_dist_valid = false;
+
+    // read any available characters from the lidar
+    int16_t nbytes = uart->available();
+    while (nbytes-- > 0) {
+        int16_t r = uart->read();
+        if (r < 0) {
+            continue;
+        }
+        if (parse_byte((uint8_t)r, latest_dist_valid, latest_dist_cm)) {
+            if (latest_dist_valid) {
+                sum_cm += latest_dist_cm;
+                count++;
+            } else {
+                count_out_of_range++;
+            }
+
+        }
+    }
+
+    if (count > 0 || count_out_of_range > 0) {
+        // record time of successful read and request another reading
+        last_distance_ms = now_ms;
+        request_distances();
+
+        if (count > 0) {
+            // return average distance of readings
+            reading_cm = sum_cm / count;
+        } else {
+            // if only out of range readings return larger of
+            // driver defined maximum range for the model and user defined max range + 1m
+            reading_cm = MAX(LEDDARVU8_DIST_MAX_CM, max_distance_cm() + LEDDARVU8_OUT_OF_RANGE_ADD_CM);
+        }
+        return true;
+    }
+
+    // no readings so return false
+    return false;
+}
+
+// get sensor address from RNGFNDx_ADDR parameter
+uint8_t AP_RangeFinder_LeddarVu8::get_sensor_address() const
+{
+    if (params.address == 0) {
+        return LEDDARVU8_ADDR_DEFAULT;
+    }
+    return params.address;
+}
+
+// send request to device to provide distances
+void AP_RangeFinder_LeddarVu8::request_distances()
+{
+    uint8_t req_buf[] = {
+            get_sensor_address(),                       // address
+            (uint8_t)FunctionCode::READ_INPUT_REGISTER, // function code low
+            0,                                          // function code high
+            (uint8_t)RegisterNumber::FIRST_DISTANCE0,   // register number low
+            0,                                          // register number high
+            8,                                          // register count
+            0,                                          // crc low
+            0                                           // crc high
+    };
+    const uint8_t req_buf_len = sizeof(req_buf);
+
+    // fill in crc bytes
+    uint16_t crc = calc_crc_modbus(req_buf, req_buf_len - 2);
+    req_buf[req_buf_len - 2] = LOWBYTE(crc);
+    req_buf[req_buf_len - 1] = HIGHBYTE(crc);
+
+    // send request to device
+    uart->write(req_buf, req_buf_len);
+
+    // record time of request
+    last_distance_request_ms = AP_HAL::millis();
+}
+
+// process one byte received on serial port
+// returns true if successfully parsed a message
+// if distances are valid, valid_readings is set to true and distance is stored in reading_cm
+bool AP_RangeFinder_LeddarVu8::parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm)
+{
+    // process byte depending upon current state
+    switch (parsed_msg.state) {
+
+    case ParseState::WAITING_FOR_ADDRESS: {
+        if (b == get_sensor_address()) {
+            parsed_msg.address = b;
+            parsed_msg.state = ParseState::WAITING_FOR_FUNCTION_CODE;
+        }
+        break;
+    }
+
+    case ParseState::WAITING_FOR_FUNCTION_CODE:
+        if (b == (uint8_t)FunctionCode::READ_INPUT_REGISTER) {
+            parsed_msg.function_code = b;
+            parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD_LEN;
+        } else {
+            parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
+        }
+        break;
+
+    case ParseState::WAITING_FOR_PAYLOAD_LEN:
+        // only parse messages of the expected length
+        if (b == LEDDARVU8_PAYLOAD_LENGTH) {
+            parsed_msg.payload_len = b;
+            parsed_msg.payload_recv = 0;
+            parsed_msg.state = ParseState::WAITING_FOR_PAYLOAD;
+        } else {
+            parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
+        }
+        break;
+
+    case ParseState::WAITING_FOR_PAYLOAD:
+        if (parsed_msg.payload_recv < parsed_msg.payload_len) {
+            if (parsed_msg.payload_recv < ARRAY_SIZE(parsed_msg.payload)) {
+                parsed_msg.payload[parsed_msg.payload_recv] = b;
+            }
+            parsed_msg.payload_recv++;
+        }
+        if (parsed_msg.payload_recv == parsed_msg.payload_len) {
+            parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
+        }
+        break;
+
+    case ParseState::WAITING_FOR_CRC_LOW:
+        parsed_msg.crc = b;
+        parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
+        break;
+
+    case ParseState::WAITING_FOR_CRC_HIGH: {
+        parsed_msg.crc |= ((uint16_t)b << 8);
+        parsed_msg.state = ParseState::WAITING_FOR_ADDRESS;
+
+        // check crc
+        uint16_t expected_crc = calc_crc_modbus(&parsed_msg.address, 3+parsed_msg.payload_recv);
+        if (expected_crc == parsed_msg.crc) {
+            // calculate and return shortest distance
+            reading_cm = 0;
+            valid_reading = false;
+            for (uint8_t i=0; i<8; i++) {
+                uint8_t ix2 = i*2;
+                const uint16_t dist_cm = (uint16_t)parsed_msg.payload[ix2] << 8 | (uint16_t)parsed_msg.payload[ix2+1];
+                if ((dist_cm > 0) && (!valid_reading || (dist_cm < reading_cm))) {
+                    reading_cm = dist_cm;
+                    valid_reading = true;
+                }
+            }
+            return true;
+        }
+        break;
+    }
+    }
+
+    valid_reading = false;
+    return false;
+}

libraries/AP_RangeFinder/AP_RangeFinder_LeddarVu8.h

@@ -0,0 +1,89 @@
+#pragma once
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+#define LEDDARVU8_PAYLOAD_LENGTH (8*2)
+
+class AP_RangeFinder_LeddarVu8 : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+protected:
+
+    // baudrate used during object construction:
+    uint32_t initial_baudrate(uint8_t serial_instance) const override {
+        return 115200;
+    }
+
+    // return sensor type as laser
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_LASER;
+    }
+
+    // get a reading, distance returned in reading_cm
+    bool get_reading(uint16_t &reading_cm) override;
+
+private:
+
+    // function codes
+    enum class FunctionCode : uint8_t {
+        READ_HOLDING_REGISTER = 0x03,
+        READ_INPUT_REGISTER = 0x04,
+        WRITE_HOLDING_REGISTER = 0x06,
+        WRITE_MULTIPLE_REGISTER = 0x10,
+        READ_WRITE_MULTIPLE_REGISTER = 0x17
+    };
+
+    // register numbers for reading input registers
+    enum class RegisterNumber : uint8_t {
+        REGISTER_STATUS = 1,    // 0 = detections not ready, 1 = ready
+        NUMBER_OF_SEGMENTS = 2,
+        NUMBER_OF_DETECTIONS = 11,
+        PERCENTAGE_OF_LIGHT_SOURCE_POWER = 12,
+        TIMESTAMP_LOW = 14,
+        TIMESTAMP_HIGH = 15,
+        FIRST_DISTANCE0 = 16,   // distance of first detection for 1st segment, zero if no detection
+        FIRST_AMPLITUDE0 = 24,  // amplitude of first detection * 64 for 1st segment
+        FIRST_FLAG0 = 32,       // flags of first detection for 1st segment.  Bit0:Valid, Bit1:Result of object demerging, Bit2:Reserved, Bit3:Saturated
+        // registers exist for distance, amplitude and flags for subsequent detections but are not used in this driver
+    };
+
+    // parsing state
+    enum class ParseState : uint8_t {
+        WAITING_FOR_ADDRESS,
+        WAITING_FOR_FUNCTION_CODE,
+        WAITING_FOR_PAYLOAD_LEN,
+        WAITING_FOR_PAYLOAD,
+        WAITING_FOR_CRC_LOW,
+        WAITING_FOR_CRC_HIGH,
+    };
+
+    // get sensor address from RNGFNDx_ADDR parameter
+    uint8_t get_sensor_address() const;
+
+    // send request to device to provide distances
+    void request_distances();
+
+    // process one byte received on serial port
+    // returns true if successfully parsed a message
+    // if distances are valid, valid_readings is set to true and distance is stored in reading_cm
+    bool parse_byte(uint8_t b, bool &valid_reading, uint16_t &reading_cm);
+
+    // structure holding latest message contents
+    // the order of fields matches the incoming message so it can be used to calculate the crc
+    struct PACKED {
+        uint8_t address;                            // device address (required for calculating crc)
+        uint8_t function_code;                      // function code (always 0x04 but required for calculating crc)
+        uint8_t payload_len;                        // message payload length
+        uint8_t payload[LEDDARVU8_PAYLOAD_LENGTH];  // payload
+        uint16_t crc;                               // latest message's crc
+        uint16_t payload_recv;                      // number of message's payload bytes received so far
+        ParseState state;                           // state of incoming message processing
+    } parsed_msg;
+    uint32_t last_distance_ms;                      // system time of last successful distance sensor read
+    uint32_t last_distance_request_ms;              // system time of last request to sensor to send distances
+};

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -6,7 +6,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: What type of rangefinder device that is connected
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:uLanding,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:BenewakeTFmini/Plus-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:UAVCAN,25:BenewakeTFmini/Plus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial
     // @User: Standard
     AP_GROUPINFO("TYPE",    1, AP_RangeFinder_Params, type, 0),