AP_RangeFinder: initial support for the DFRobot07 lidar

Tools/scripts/build_options.py

@@ -247,6 +247,7 @@ def config_option(self):
     Feature('Rangefinder', 'RFND_BENEWAKE_TFMINI', 'AP_RANGEFINDER_BENEWAKE_TFMINI_ENABLED', "Enable Rangefinder - Benewake - TFMini", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RFND_BENEWAKE_TFMINIPLUS', 'AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED', "Enable Rangefinder - Benewake - TFMiniPlus", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_BLPING', 'AP_RANGEFINDER_BLPING_ENABLED', "Enable Rangefinder - BLPing", 0, "RANGEFINDER"),   # NOQA: E501
+    Feature('Rangefinder', 'RANGEFINDER_DFROBOT_LIDAR07', 'AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED', "Enable Rangefinder - DFRobot Lidar07", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_GYUS42V2', 'AP_RANGEFINDER_GYUS42V2_ENABLED', "Enable Rangefinder - GYUS42V2", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_HC_SR04', 'AP_RANGEFINDER_HC_SR04_ENABLED', "Enable Rangefinder - HC_SR04", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_JRE_SERIAL', 'AP_RANGEFINDER_JRE_SERIAL_ENABLED', "Enable Rangefinder - JRE_SERIAL", 0, "RANGEFINDER"),   # NOQA: E501

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -64,6 +64,7 @@
 #include "AP_RangeFinder_JRE_Serial.h"
 #include "AP_RangeFinder_Ainstein_LR_D1.h"
 #include "AP_RangeFinder_RDS02UF.h"
+#include "AP_RangeFinder_DFRobot_Lidar07.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -598,6 +599,11 @@ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial_instance)
     case Type::RDS02UF:
         serial_create_fn = AP_RangeFinder_RDS02UF::create;
         break;
+#endif
+#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED
+    case Type::DFRobot_Lidar07:
+        serial_create_fn = AP_RangeFinder_DFRobot_Lidar07::create;
+        break;
 #endif
     case Type::NONE:
         break;

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -182,6 +182,9 @@ class RangeFinder
 #if AP_RANGEFINDER_RDS02UF_ENABLED
         RDS02UF = 43,
 #endif
+#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED
+        DFRobot_Lidar07 = 44,
+#endif
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.cpp

@@ -0,0 +1,217 @@
+/*
+   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_HAL/utility/sparse-endian.h"
+#include "AP_RangeFinder_config.h"
+#include <endian.h>
+
+#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED
+
+#include "AP_RangeFinder_DFRobot_Lidar07.h"
+
+// DFRobot Lidar07 support driver
+// Also sold as VSemi Atom
+// Documentation: https://dfimg.dfrobot.com/nobody/wiki/1840a7b7b14e02f3566e0cef5b51e9ba.pdf
+// https://wiki.dfrobot.com/TOF_IR_Distance_Sensor_0.2_12m_SKU_SEN0413
+// 
+// Current implementation only is limited to:
+// - UART interface
+// - Basic distance reading in continuous mode without an inbuilt filter
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/crc.h>
+extern const AP_HAL::HAL& hal;
+
+#define LIDAR07_SOF_SENSOR      (0xfa)
+#define LIDAR07_SOF_CONTROLLER  (0xf5)
+#define LIDAR07_WRITE_MASK      (0x80)
+#define LIDAR07_REG_VERSION     (0x43)
+#define LIDAR07_REG_FILTER      (0x59)
+#define LIDAR07_REG_MEASUREMENT (0x60)
+#define LIDAR07_REG_MODE        (0x61)
+#define LIDAR07_REG_INTERVAL    (0x62)
+#define LIDAR07_REG_ERROR       (0x65)
+
+// the sensor supports rates up to 100 Hz
+// use the lowest common rate between Copter and Plane: 20 Hz = 50 ms
+#define LIDAR07_FRAME_INTERVAL_MS       (50)
+
+// Invalidate the temperature and other readings after 2 measurement intervals
+#define LIDAR07_DATA_EXPIRATION_MS      (2*LIDAR07_FRAME_INTERVAL_MS)
+
+// Retry initialization if inactive for 30 seconds
+#define LIDAR07_STALE_CONNECTION_MS     (30*1000)
+
+// DFRobot's CRC32 implementation uses unreflected bit order
+// apparently it's not the way CRC32 is used elsewhere so it makes sense to hide
+// this utility under the define block
+template <typename T> T reflect(const T& p)
+{
+    // NOTE: straightforward bit reflection, can probably be optimized if used in high frequency code
+    T res = 0;
+    size_t bit_len = sizeof(T) * 8;
+    for (size_t i = 0; i < bit_len; i++) {
+        res |= bool(p & (1<<i)) << (bit_len - i - 1);
+    }
+    return res;
+}
+
+uint32_t AP_RangeFinder_DFRobot_Lidar07::crc32_unreflected(uint8_t *buf, size_t n)
+{
+    uint8_t ubuf[n];
+    for (size_t i = 0; i < n; i++)
+        ubuf[i] = reflect<uint8_t>(buf[i]);
+    return reflect<uint32_t>(crc_crc32(0xffffffff, ubuf, n));
+}
+
+void AP_RangeFinder_DFRobot_Lidar07::prepare_command(uint8_t reg, uint32_t value)
+{
+    _packet.start = LIDAR07_SOF_CONTROLLER;
+    _packet.reg = reg;
+    _packet.command.value = htole32(value);
+    _packet.command.checksum = htole32(crc32_unreflected(_buf, offsetof(Packet, command.checksum)));
+    _buf_pos = 0;
+    _writing = true;
+    _exp_reg = reg;
+}
+
+bool AP_RangeFinder_DFRobot_Lidar07::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // are we coming out of reset?
+    if (!_exp_reg) {
+        // no low noise filter to speed things up
+        prepare_command(LIDAR07_REG_FILTER | LIDAR07_WRITE_MASK, 0);
+    }
+
+    // we have data to write, send what we can and return to get rescheduled
+    if (_writing) {
+        size_t left = offsetof(Packet, command.checksum) + sizeof(Packet::command.checksum) - _buf_pos;
+        size_t written = uart->write(&_buf[_buf_pos], left);
+        if (written < left) {
+            _buf_pos += written;
+        } else {
+            _buf_pos = 0;
+            _writing = false;
+        }
+
+        return false;
+    }
+
+    // invalidate connection and restart the setup if no data received in a while
+    if (AP_HAL::millis() - _last_read_ms > LIDAR07_STALE_CONNECTION_MS) {
+        _exp_reg = 0;
+        return false;
+    }
+
+    // try reading a whole message from the serial
+    for (size_t i = 0; i < sizeof(_buf); i++) {
+        uint8_t c;
+        if (!uart->read(c)) {
+            break;
+        }
+        _last_read_ms = AP_HAL::millis();
+
+        // restart if we are somehow out of buffer
+        if (_buf_pos == sizeof(_buf)) {
+            _buf_pos = 0;
+            continue;
+        }
+
+        // reject spurious fragments
+        if (_buf_pos == offsetof(Packet, start) && c != LIDAR07_SOF_SENSOR) {
+            continue;
+        }
+
+        // reject things we aren't expecting to get
+        if (_buf_pos == offsetof(Packet, reg) && c != _exp_reg) {
+            _buf_pos = 0;
+            continue;
+        }
+
+        _buf[_buf_pos++] = c;
+
+        // pull the next byte if we haven't read the length yet
+        constexpr size_t data_offset = offsetof(Packet, len) + sizeof(Packet::len);
+        if (_buf_pos < data_offset) {
+            continue;
+        }
+
+        // check if data length makes sense after we read all of the value
+        size_t exp_len = _exp_reg == LIDAR07_REG_MEASUREMENT? sizeof(Packet::measurement) : sizeof(Packet::responce);
+        if (_buf_pos == data_offset && le16toh(_packet.len) != exp_len) {
+            _buf_pos = 0;
+            continue;
+        }
+
+        uint32_t crc32_offset = LIDAR07_REG_MEASUREMENT? offsetof(Packet, measurement.checksum) : offsetof(Packet, responce.checksum);
+        // looks like we have a full message, reset the counter and verify it
+        if (_buf_pos >= crc32_offset + sizeof(uint32_t)) {
+            _buf_pos = 0;
+
+            // match the checksums
+            // incoming value on the wire is little endian
+            uint32_t message_crc32 = crc32_unreflected(_buf, crc32_offset);
+            uint32_t incoming_crc32 = LIDAR07_REG_MEASUREMENT? _packet.measurement.checksum : _packet.responce.checksum;
+            if (le32toh(incoming_crc32) == message_crc32) {
+                // We have a valid message, act in accordance to what we are expecting
+                // NOTE: we aren't actually checking that the responce values match what we demanded
+                switch (_exp_reg) {
+                    case LIDAR07_REG_FILTER: 
+                        // continuous mode
+                        prepare_command(LIDAR07_REG_MODE | LIDAR07_WRITE_MASK, 1);
+                        return false;
+
+                    case LIDAR07_REG_MODE:
+                        // frame rate
+                        prepare_command(LIDAR07_REG_INTERVAL | LIDAR07_WRITE_MASK, LIDAR07_FRAME_INTERVAL_MS);
+                        return false;
+
+                    case LIDAR07_REG_INTERVAL:
+                        // start measuring
+                        prepare_command(LIDAR07_REG_MEASUREMENT | LIDAR07_WRITE_MASK, 1);
+                        return false;
+
+                    case LIDAR07_REG_MEASUREMENT:
+                        // got a valid measurement, capture the fields
+                        reading_m = le16toh(_packet.measurement.distance)/1000.0;
+                        _temperature = le16toh(_packet.measurement.temperature)/100.0;
+
+                        // NOTE: other fields are unused right now
+                        // we can probably figure out signal quality by light level/amplitude
+                        return true;
+                }
+            }
+        }
+    }
+
+    // we've got nothing at this time
+    return false;
+}
+
+bool AP_RangeFinder_DFRobot_Lidar07::get_temp(float &temp) const
+{
+    if (AP_HAL::millis() - _last_read_ms > LIDAR07_DATA_EXPIRATION_MS) {
+        return false;
+    }
+
+    temp = _temperature;
+    return true;
+}
+
+#endif  // AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_DFRobot_Lidar07.h

@@ -0,0 +1,130 @@
+/*
+   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/>.
+ */
+
+#pragma once
+
+#include "AP_RangeFinder_config.h"
+
+#if AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED
+
+#include "AP_RangeFinder.h"
+#include "AP_RangeFinder_Backend_Serial.h"
+
+#include <AP_HAL/utility/sparse-endian.h>
+
+class AP_RangeFinder_DFRobot_Lidar07 : public AP_RangeFinder_Backend_Serial
+{
+
+public:
+
+    static AP_RangeFinder_Backend_Serial *create(
+        RangeFinder::RangeFinder_State &_state,
+        AP_RangeFinder_Params &_params) {
+        return NEW_NOTHROW AP_RangeFinder_DFRobot_Lidar07(_state, _params);
+    }
+
+protected:
+
+    // 850 nm LED
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override {
+        return MAV_DISTANCE_SENSOR_INFRARED;
+    }
+
+private:
+
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    // 115200 baud as per datasheet
+    uint32_t initial_baudrate(uint8_t serial_instance) const override { return 115200; }
+
+    // get a distance reading
+    bool get_reading(float &reading_m) override;
+
+    // get the latest encountered temperature
+    bool get_temp(float &temp) const override;
+
+    // generate a command to the sensor and place it in the message buffer
+    void prepare_command(uint8_t reg, uint32_t value);
+
+    // unreflected crc32 checksum with 0xffffffff initial value
+    static uint32_t crc32_unreflected(uint8_t *buf, size_t n);
+
+    // last temperature seen
+    float _temperature;
+
+    // packet structure
+    struct Packet
+    {
+        uint8_t start;
+        uint8_t reg;
+        union
+        {
+            // single value for packets sent to the sensor
+            struct 
+            {
+                le32_t value;
+                le32_t checksum;
+            } command __attribute__((packed));
+            // sensor responce
+            struct
+            {
+                // responces include a 16-bit data length although only 2 values are ever used
+                le16_t len;
+                union
+                {
+                    // generic 32-bit reply
+                    struct
+                    {
+                        le32_t value;
+                        le32_t checksum;
+                    } responce __attribute__((packed));
+                    // measurement response
+                    struct
+                    {
+                        le16_t distance;
+                        le16_t temperature;
+                        le16_t amplitude;
+                        le16_t ambient_light;
+                        le64_t tof_phase;
+                        le32_t checksum;
+                    } measurement __attribute__((packed));
+                };
+            } __attribute__((packed));
+        };
+
+    } __attribute__((packed));
+
+    // one message buffer
+    union
+    {
+        Packet _packet;
+        uint8_t _buf[sizeof(Packet)];
+    };
+
+    // current buffer position
+    size_t _buf_pos = 0;
+
+    // whether we are reading or writing
+    bool _writing = false;
+
+    // expected message register
+    // 0 carries a special meaning of a state after reset
+    uint8_t _exp_reg = 0;
+
+    // last time we've received a valid reading
+    uint32_t _last_read_ms = 0;
+};
+
+#endif  // AP_RANGEFINDER_DFROBOT_LIDAR07_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -14,7 +14,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Rangefinder type
     // @Description: Type of connected rangefinder
-    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,100:SITL
+    // @Values: 0:None,1:Analog,2:MaxbotixI2C,3:LidarLite-I2C,5:PWM,6:BBB-PRU,7:LightWareI2C,8:LightWareSerial,9:Bebop,10:MAVLink,11:USD1_Serial,12:LeddarOne,13:MaxbotixSerial,14:TeraRangerI2C,15:LidarLiteV3-I2C,16:VL53L0X or VL53L1X,17:NMEA,18:WASP-LRF,19:BenewakeTF02,20:Benewake-Serial,21:LidarLightV3HP,22:PWM,23:BlueRoboticsPing,24:DroneCAN,25:BenewakeTFminiPlus-I2C,26:LanbaoPSK-CM8JL65-CC5,27:BenewakeTF03,28:VL53L1X-ShortRange,29:LeddarVu8-Serial,30:HC-SR04,31:GYUS42v2,32:MSP,33:USD1_CAN,34:Benewake_CAN,35:TeraRangerSerial,36:Lua_Scripting,37:NoopLoop_TOFSense,38:NoopLoop_TOFSense_CAN,39:NRA24_CAN,40:NoopLoop_TOFSenseF_I2C,41:JRE_Serial,42:Ainstein_LR_D1,43:RDS02UF,44:DFrobot_Lidar07,100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),