AP_RangeFinder: add DTS6012M driver support

FractalEngineer · FractalEngineer · commit ed3b139e3d0d · 2026-02-13T22:05:43.000+08:00
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/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
+
     case Type::NONE:
         break;
     }
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/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
 #if AP_RANGEFINDER_SIM_ENABLED
         SIM = 100,
 #endif
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.cpp
@@ -0,0 +1,192 @@
+/*
+   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_DTS6012M.h"
+
+#if AP_RANGEFINDER_DTS6012M_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+
+extern const AP_HAL::HAL& hal;
+
+/*
+  CRC-16/MODBUS calculation
+  Polynomial: 0x8005, Init: 0xFFFF, RefIn: true, RefOut: true, XorOut: 0x0000
+*/
+uint16_t AP_RangeFinder_DTS6012M::crc16_modbus(const uint8_t *data, uint16_t len)
+{
+    uint16_t crc = 0xFFFF;
+    for (uint16_t i = 0; i < len; i++) {
+        crc ^= data[i];
+        for (uint8_t j = 0; j < 8; j++) {
+            if (crc & 0x0001) {
+                crc = (crc >> 1) ^ 0xA001;  // 0xA001 is the bit-reversed form of 0x8005
+            } else {
+                crc >>= 1;
+            }
+        }
+    }
+    return crc;
+}
+
+/*
+  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()
+{
+    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 = crc16_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));
+}
+
+/*
+  read from the sensor and return distance in meters
+  Protocol response frame (23 bytes total):
+    Header: A5 03 20 01 00 00 0E  (7 bytes)
+    Data (14 bytes):
+      [0-1]  Secondary target distance (LE, mm) - 0xFFFF if invalid
+      [2-3]  Secondary target correction
+      [4-5]  Secondary target intensity
+      [6-7]  Primary target distance (LE, mm)
+      [8-9]  Primary target correction
+      [10-11] Primary target intensity
+      [12-13] Sunlight base
+    CRC16: 2 bytes (big-endian)
+*/
+bool AP_RangeFinder_DTS6012M::get_reading(float &reading_m)
+{
+    if (uart == nullptr) {
+        return false;
+    }
+
+    // send start command on first call
+    if (!start_cmd_sent) {
+        send_start_command();
+        start_cmd_sent = true;
+    }
+
+    float sum_mm = 0;
+    uint16_t count = 0;
+    uint16_t count_out_of_range = 0;
+    int32_t latest_intensity = -1;
+
+    // read available bytes from the sensor
+    for (auto i = 0; i < 8192; i++) {
+        uint8_t c;
+        if (!uart->read(c)) {
+            break;
+        }
+
+        // waiting for frame header
+        if (linebuf_len == 0) {
+            if (c == DTS6012M_FRAME_HEADER) {
+                linebuf[linebuf_len++] = c;
+            }
+            continue;
+        }
+
+        // accumulate bytes into buffer
+        linebuf[linebuf_len++] = c;
+
+        // validate header fields as they arrive
+        if (linebuf_len == 2 && c != DTS6012M_DEVICE_ID) {
+            linebuf_len = 0;
+            continue;
+        }
+        if (linebuf_len == 3 && c != DTS6012M_DEVICE_TYPE) {
+            linebuf_len = 0;
+            continue;
+        }
+        if (linebuf_len == 4 && c != DTS6012M_CMD_START_STREAM) {
+            // not a measurement frame, discard
+            linebuf_len = 0;
+            continue;
+        }
+
+        // wait for complete frame
+        if (linebuf_len < DTS6012M_FRAME_LEN) {
+            continue;
+        }
+
+        // frame complete - verify length field
+        const uint16_t data_len = (uint16_t(linebuf[5]) << 8) | linebuf[6];
+        if (data_len != DTS6012M_DATA_LEN) {
+            linebuf_len = 0;
+            continue;
+        }
+
+        // verify CRC-16 over header + data (all bytes except last 2)
+        const uint16_t crc_calc = crc16_modbus(linebuf, DTS6012M_FRAME_LEN - DTS6012M_CRC_LEN);
+        const uint16_t crc_recv = (uint16_t(linebuf[DTS6012M_FRAME_LEN - 2]) << 8) | linebuf[DTS6012M_FRAME_LEN - 1];
+
+        if (crc_calc != crc_recv) {
+            linebuf_len = 0;
+            continue;
+        }
+
+        // extract primary target distance (data bytes 6-7, little-endian, in mm)
+        const uint8_t *data = &linebuf[DTS6012M_HEADER_LEN];
+        const uint16_t dist_mm = uint16_t(data[7]) << 8 | data[6];
+
+        if (dist_mm == DTS6012M_DIST_INVALID) {
+            count_out_of_range++;
+        } else if (dist_mm > DTS6012M_DIST_MAX_MM) {
+            count_out_of_range++;
+        } else {
+            sum_mm += dist_mm;
+            count++;
+        }
+
+        // capture primary target intensity for signal quality (data bytes 10-11, LE)
+        latest_intensity = int32_t(uint16_t(data[11]) << 8 | data[10]);
+
+        // reset for next frame
+        linebuf_len = 0;
+    }
+
+    // update signal quality from latest intensity reading
+    if (latest_intensity >= 0) {
+        // map intensity to 0-100%, clamping at 10000 as practical maximum
+        _signal_quality_pct = int8_t(constrain_int32(latest_intensity * 100 / 10000, 0, 100));
+    }
+
+    if (count > 0) {
+        reading_m = (sum_mm * 0.001f) / count;
+        return true;
+    }
+
+    if (count_out_of_range > 0) {
+        reading_m = max_distance() + 1.0f;
+        return true;
+    }
+
+    return false;
+}
+
+#endif  // AP_RANGEFINDER_DTS6012M_ENABLED
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h b/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h
@@ -0,0 +1,71 @@
+/*
+   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"
+
+// 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_CMD_STOP_STREAM    0x02
+#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
+
+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
+    uint8_t linebuf[DTS6012M_FRAME_LEN];
+    uint8_t linebuf_len = 0;
+    bool start_cmd_sent = false;
+    int8_t _signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
+
+    // send start stream command to sensor
+    void send_start_command();
+
+    // CRC-16/MODBUS calculation
+    static uint16_t crc16_modbus(const uint8_t *data, uint16_t len);
+};
+
+#endif  // AP_RANGEFINDER_DTS6012M_ENABLED
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/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),
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_config.h b/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
+#endif
+
 #define AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED (AP_RANGEFINDER_HEXSOONRADAR_ENABLED || AP_RANGEFINDER_NRA24_CAN_ENABLED)
