diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py index 1b2cd47f100de..27412f05172fa 100644 --- a/Tools/autotest/arducopter.py +++ b/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. diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index d98b38533afce..f59cdce852332 100644 --- a/Tools/scripts/build_options.py +++ b/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 diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp index 80bc365383fb1..acb74cbe3e74c 100644 --- a/libraries/AP_HAL_SITL/SITL_State_common.cpp +++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -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 }, diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.cpp b/libraries/AP_RangeFinder/AP_RangeFinder.cpp index a95df3ebe34b8..749f637cdb9e7 100644 --- 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 #include @@ -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; } diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h index bfed63569dd1a..aa57b8cc18917 100644 --- 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 // AP_RANGEFINDER_DTS6012M_ENABLED #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 new file mode 100644 index 0000000000000..1f7071caed0bf --- /dev/null +++ b/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 . + */ + + +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_DTS6012M_ENABLED + +#include "AP_RangeFinder_DTS6012M.h" +#include +#include + +// 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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h b/libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h new file mode 100644 index 0000000000000..92cd1675d19fa --- /dev/null +++ b/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 . + */ + +#pragma once + +#include "AP_RangeFinder_config.h" + +#if AP_RANGEFINDER_DTS6012M_ENABLED + +#include "AP_RangeFinder_Backend_Serial.h" +#include + + +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 diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp index abeac3ccbbf89..91a4ab1c09bf0 100644 --- 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 index 1011f0c1d304b..45a680ec4af60 100644 --- 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 && HAL_PROGRAM_SIZE_LIMIT_KB > 1024) +#endif + #define AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED (AP_RANGEFINDER_HEXSOONRADAR_ENABLED || AP_RANGEFINDER_NRA24_CAN_ENABLED) diff --git a/libraries/SITL/SIM_RF_DTS6012M.cpp b/libraries/SITL/SIM_RF_DTS6012M.cpp new file mode 100644 index 0000000000000..8bf1459a41ae5 --- /dev/null +++ b/libraries/SITL/SIM_RF_DTS6012M.cpp @@ -0,0 +1,72 @@ +/* + 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 . + */ +/* + Simulator for the DTS6012M serial rangefinder +*/ + +#include "SIM_config.h" + +#if AP_SIM_RF_DTS6012M_ENABLED + +#include "SIM_RF_DTS6012M.h" +#include + +using namespace SITL; + +uint32_t RF_DTS6012M::packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen) +{ + const uint8_t PACKET_LEN = 23; + + if (buflen < PACKET_LEN) { + abort(); + } + + // sensor sends 0xFFFF when out of range (confirmed on hardware) + const uint16_t dist_mm = (alt_m * 1000 > 20000) ? 0xFFFF : (uint16_t)(alt_m * 1000); + + // 7-byte header + buffer[0] = 0xA5; // frame header + buffer[1] = 0x03; // device ID + buffer[2] = 0x20; // device type + buffer[3] = 0x01; // command echo (start stream) + buffer[4] = 0x00; // reserved + buffer[5] = 0x00; // data length high byte (14 = 0x000E, big-endian) + buffer[6] = 0x0E; // data length low byte + + // 14-byte data payload + buffer[7] = 0xFF; // secondary target distance low byte (0xFFFF = invalid) + buffer[8] = 0xFF; // secondary target distance high byte + buffer[9] = 0x00; // secondary target correction low + buffer[10] = 0x00; // secondary target correction high + buffer[11] = 0x00; // secondary target intensity low + buffer[12] = 0x00; // secondary target intensity high + buffer[13] = dist_mm & 0xFF; // primary target distance low byte (little-endian, mm) + buffer[14] = dist_mm >> 8; // primary target distance high byte + buffer[15] = 0x00; // primary target correction low + buffer[16] = 0x00; // primary target correction high + buffer[17] = 0x10; // primary target intensity low byte (10000 = 0x2710 → 100% quality) + buffer[18] = 0x27; // primary target intensity high byte + buffer[19] = 0x00; // sunlight base low + buffer[20] = 0x00; // sunlight base high + + // CRC-16/MODBUS over bytes 0–20, stored big-endian (high byte first) + const uint16_t crc = calc_crc_modbus(buffer, PACKET_LEN - 2); + buffer[21] = crc >> 8; // CRC high byte + buffer[22] = crc & 0xFF; // CRC low byte + + return PACKET_LEN; +} + +#endif // AP_SIM_RF_DTS6012M_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_RF_DTS6012M.h b/libraries/SITL/SIM_RF_DTS6012M.h new file mode 100644 index 0000000000000..6083268b79c13 --- /dev/null +++ b/libraries/SITL/SIM_RF_DTS6012M.h @@ -0,0 +1,55 @@ +/* + 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 . + */ +/* + Simulator for the DTS6012M serial rangefinder + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --serial5=sim:dts6012m --speedup=1 + +param set SERIAL5_PROTOCOL 9 +param set RNGFND1_TYPE 47 +graph RANGEFINDER.distance +graph GLOBAL_POSITION_INT.relative_alt/1000-RANGEFINDER.distance +reboot + +arm throttle +rc 3 1600 + +*/ + +#pragma once + +#include "SIM_config.h" + +#if AP_SIM_RF_DTS6012M_ENABLED + +#include "SIM_SerialRangeFinder.h" + + +namespace SITL { + +class RF_DTS6012M : public SerialRangeFinder { +public: + + static SerialRangeFinder *create() { return NEW_NOTHROW RF_DTS6012M(); } + + uint32_t packet_for_alt(float alt_m, uint8_t *buffer, uint8_t buflen) override; + + uint16_t reading_interval_ms() const override { return 100; } // (Downsampled to 100 for 10Hz, sensor capable of 10 for 100Hz) + +}; + +} + +#endif // AP_SIM_RF_DTS6012M_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 385191a517102..beb80e5d31088 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -245,6 +245,10 @@ #define AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // AP_SIM_RF_BENEWAKE_TFMINIPLUS_ENABLED +#ifndef AP_SIM_RF_DTS6012M_ENABLED +#define AP_SIM_RF_DTS6012M_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // AP_SIM_RF_DTS6012M_ENABLED + #ifndef AP_SIM_RAMTRON_ENABLED #define AP_SIM_RAMTRON_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) #endif // AP_SIM_RAMTRON_ENABLED