AP_RangeFinder: add DTS6012M driver support

Tools/autotest/arducopter.py

@@ -10593,6 +10593,7 @@ def RangeFinderDrivers(self):
             ("ainsteinlrd1", 42),
             ("rds02uf", 43),
             ("lightware_grf", 45),
+            ("dts6012m", 47),
         ]
         # you can use terrain - if you don't the vehicle just uses a
         # plane based on home.

Tools/scripts/build_options.py

@@ -300,6 +300,7 @@ def config_option(self):
     Feature('Rangefinder', 'RFND_BENEWAKE_TFMINIPLUS', 'AP_RANGEFINDER_BENEWAKE_TFMINIPLUS_ENABLED', "Enable Rangefinder - Benewake - TFMiniPlus", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RFND_BENEWAKE_TFS20L', 'AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED', "Enable Rangefinder - Benewake - TFS20L", 0, "RANGEFINDER"),   # NOQA: E501
     Feature('Rangefinder', 'RANGEFINDER_BLPING', 'AP_RANGEFINDER_BLPING_ENABLED', "Enable Rangefinder - BLPing", 0, "RANGEFINDER"),   # NOQA: E501
+    Feature('Rangefinder', 'RANGEFINDER_DTS6012M', 'AP_RANGEFINDER_DTS6012M_ENABLED', "Enable Rangefinder - DTS6012M", 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_HAL_SITL/SITL_State_common.cpp

@@ -27,6 +27,7 @@
 #include <SITL/SIM_RF_Benewake_TF03.h>
 #include <SITL/SIM_RF_Benewake_TFmini.h>
 #include <SITL/SIM_RF_BLping.h>
+#include <SITL/SIM_RF_DTS6012M.h>
 #include <SITL/SIM_RF_GYUS42v2.h>
 #include <SITL/SIM_RF_JRE.h>
 #include <SITL/SIM_RF_Lanbao.h>
@@ -56,6 +57,7 @@ static const struct {
     { "benewake_tf03", SITL::RF_Benewake_TF03::create },
     { "benewake_tfmini", SITL::RF_Benewake_TFmini::create },
     { "blping", SITL::RF_BLping::create },
+    { "dts6012m", SITL::RF_DTS6012M::create },
     { "gyus42v2", SITL::RF_GYUS42v2::create },
     { "jre", SITL::RF_JRE::create },
     { "lanbao", SITL::RF_Lanbao::create },

libraries/AP_RangeFinder/AP_RangeFinder.cpp

@@ -64,6 +64,7 @@
 #include "AP_RangeFinder_Ainstein_LR_D1.h"
 #include "AP_RangeFinder_RDS02UF.h"
 #include "AP_RangeFinder_LightWare_GRF.h"
+#include "AP_RangeFinder_DTS6012M.h"
 
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_Logger/AP_Logger.h>
@@ -628,6 +629,12 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
         break;
 #endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
 
+#if AP_RANGEFINDER_DTS6012M_ENABLED
+    case Type::DTS6012M:
+        serial_create_fn = AP_RangeFinder_DTS6012M::create;
+        break;
+#endif // AP_RANGEFINDER_DTS6012M_ENABLED
+
     case Type::NONE:
         break;
     }

libraries/AP_RangeFinder/AP_RangeFinder.h

@@ -191,6 +191,9 @@ class RangeFinder
 #if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
         BenewakeTFS20L = 46,
 #endif // AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
+#if AP_RANGEFINDER_DTS6012M_ENABLED
+        DTS6012M = 47,
+#endif // AP_RANGEFINDER_DTS6012M_ENABLED
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif

libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.cpp

@@ -0,0 +1,167 @@
+/*
+   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_config.h"
+
+#if AP_RANGEFINDER_DTS6012M_ENABLED
+
+#include "AP_RangeFinder_DTS6012M.h"
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Math/crc.h>
+
+// DTS6012M protocol constants
+#define DTS6012M_FRAME_HEADER       0xA5
+#define DTS6012M_DEVICE_ID          0x03
+#define DTS6012M_DEVICE_TYPE        0x20
+#define DTS6012M_CMD_START_STREAM   0x01
+#define DTS6012M_HEADER_LEN         7       // header(1) + devid(1) + devtype(1) + cmd(1) + reserved(1) + length(2)
+#define DTS6012M_DATA_LEN           14      // measurement data length
+#define DTS6012M_CRC_LEN            2
+#define DTS6012M_FRAME_LEN          (DTS6012M_HEADER_LEN + DTS6012M_DATA_LEN + DTS6012M_CRC_LEN)  // 23 bytes
+#define DTS6012M_DIST_MAX_MM        20000   // 20m max range
+#define DTS6012M_DIST_INVALID       0xFFFF
+
+// set to 0 to disable CRC verification if the sensor's CRC proves unreliable
+#ifndef DTS6012M_CRC_ENABLE
+#define DTS6012M_CRC_ENABLE         1
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  send the start stream command (0x01) to begin periodic measurement output
+  Frame: A5 03 20 01 00 00 00 CRC16_H CRC16_L
+*/
+void AP_RangeFinder_DTS6012M::send_start_command()
+{
+    static const uint8_t cmd[] = {
+        DTS6012M_FRAME_HEADER,  // 0xA5
+        DTS6012M_DEVICE_ID,     // 0x03
+        DTS6012M_DEVICE_TYPE,   // 0x20
+        DTS6012M_CMD_START_STREAM,  // 0x01
+        0x00,                   // reserved
+        0x00, 0x00              // length = 0 (no data)
+    };
+
+    // calculate CRC over the command bytes
+    const uint16_t crc = calc_crc_modbus(cmd, sizeof(cmd));
+
+    uart->write(cmd, sizeof(cmd));
+    // CRC is sent high byte first per protocol spec
+    uart->write(uint8_t(crc >> 8));
+    uart->write(uint8_t(crc & 0xFF));
+}
+
+/*
+  search for frame header byte in buffer starting at offset,
+  shifting remaining data to re-sync after line noise
+*/
+void AP_RangeFinder_DTS6012M::find_signature_in_buffer(uint8_t start)
+{
+    uint8_t *buf = (uint8_t *)&frame;
+    for (uint8_t i = start; i < linebuf_len; i++) {
+        if (buf[i] == DTS6012M_FRAME_HEADER) {
+            memmove(&buf[0], &buf[i], linebuf_len - i);
+            linebuf_len -= i;
+            return;
+        }
+    }
+    linebuf_len = 0;
+}
+
+/*
+  read from the sensor and return distance in meters
+  see frame struct in header for protocol layout
+*/
+bool AP_RangeFinder_DTS6012M::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // keep sending start command until we receive a valid reading,
+    // allowing for slow-startup or late connection of the device
+    if (!got_reading) {
+        send_start_command();
+    }
+
+    // bulk read available bytes into buffer
+    uint8_t *buf = (uint8_t *)&frame;
+    const auto num_read = uart->read(&buf[linebuf_len], sizeof(frame) - linebuf_len);
+    linebuf_len += num_read;
+
+    if (linebuf_len == 0) {
+        return false;
+    }
+
+    // ensure buffer starts with frame header
+    if (frame.header != DTS6012M_FRAME_HEADER) {
+        find_signature_in_buffer(1);
+        return false;
+    }
+
+    // wait for a complete frame
+    if (linebuf_len < DTS6012M_FRAME_LEN) {
+        return false;
+    }
+
+    // validate header fields
+    if (frame.device_id != DTS6012M_DEVICE_ID ||
+        frame.device_type != DTS6012M_DEVICE_TYPE ||
+        frame.cmd_echo != DTS6012M_CMD_START_STREAM) {
+        find_signature_in_buffer(1);
+        return false;
+    }
+
+    // verify length field
+    if (be16toh(frame.data_len) != DTS6012M_DATA_LEN) {
+        find_signature_in_buffer(1);
+        return false;
+    }
+
+    // verify CRC-16 over header + data (all bytes except last 2)
+#if DTS6012M_CRC_ENABLE
+    const uint16_t crc_calc = calc_crc_modbus(buf, DTS6012M_FRAME_LEN - DTS6012M_CRC_LEN);
+    if (crc_calc != be16toh(frame.crc)) {
+        find_signature_in_buffer(1);
+        return false;
+    }
+#endif
+
+    // extract primary target distance (little-endian, in mm)
+    const uint16_t dist_mm = le16toh(frame.primary_distance_mm);
+
+    // capture primary target intensity for signal quality
+    const int32_t intensity = int32_t(le16toh(frame.primary_intensity));
+    // map intensity to 0-100%, clamping at 10000 as practical maximum
+    _signal_quality_pct = int8_t(constrain_int32(intensity * 100 / 10000, 0, 100));
+
+    // frame consumed, reset buffer and discard any stale data so the
+    // next call reads the freshest frame from this high-rate sensor
+    linebuf_len = 0;
+    uart->discard_input();
+    got_reading = true;
+
+    if (dist_mm == DTS6012M_DIST_INVALID || dist_mm > DTS6012M_DIST_MAX_MM) {
+        reading_m = max_distance() + 1.0f;
+        return true;
+    }
+
+    reading_m = dist_mm * 0.001f;
+    return true;
+}
+
+#endif  // AP_RANGEFINDER_DTS6012M_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h

@@ -0,0 +1,78 @@
+/*
+   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_DTS6012M_ENABLED
+
+#include "AP_RangeFinder_Backend_Serial.h"
+#include <AP_HAL/utility/sparse-endian.h>
+
+
+class AP_RangeFinder_DTS6012M : 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_DTS6012M(_state, _params);
+    }
+
+protected:
+    using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
+
+    MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_LASER; }
+
+private:
+    bool get_reading(float &reading_m) override;
+    uint32_t initial_baudrate(uint8_t serial_instance) const override { return 921600; }
+    int8_t get_signal_quality_pct() const override { return _signal_quality_pct; }
+
+    // protocol parsing state
+    // response frame layout matching the DTS6012M wire format
+    struct PACKED {
+        uint8_t header;         // 0xA5
+        uint8_t device_id;      // 0x03
+        uint8_t device_type;    // 0x20
+        uint8_t cmd_echo;       // 0x01
+        uint8_t reserved;
+        be16_t data_len;        // big-endian length
+        // measurement data (14 bytes)
+        le16_t secondary_distance_mm;
+        le16_t secondary_correction;
+        le16_t secondary_intensity;
+        le16_t primary_distance_mm;
+        le16_t primary_correction;
+        le16_t primary_intensity;
+        le16_t sunlight_base;
+        // CRC16 (big-endian)
+        be16_t crc;
+    } frame;
+    uint8_t linebuf_len;
+    bool got_reading;
+    int8_t _signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
+
+    // send start stream command to sensor
+    void send_start_command();
+
+    // search for frame header byte starting at given offset,
+    // shifting buffer contents to re-sync after line noise
+    void find_signature_in_buffer(uint8_t start);
+};
+
+#endif  // AP_RANGEFINDER_DTS6012M_ENABLED

libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp

@@ -61,6 +61,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
     // @Values: 44:HexsoonRadar
     // @Values: 45:LightWare-GRF
     // @Values: 46:BenewakeTFS20L
+    // @Values: 47:DTS6012M
     // @Values: 100:SITL
     // @User: Standard
     AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),

libraries/AP_RangeFinder/AP_RangeFinder_config.h

@@ -199,4 +199,8 @@
 #define AP_RANGEFINDER_HEXSOONRADAR_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
 #endif
 
+#ifndef AP_RANGEFINDER_DTS6012M_ENABLED
+#define AP_RANGEFINDER_DTS6012M_ENABLED (AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024)
+#endif
+
 #define AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED (AP_RANGEFINDER_HEXSOONRADAR_ENABLED || AP_RANGEFINDER_NRA24_CAN_ENABLED)