Skip to content

Commit c234be2

Browse files
FractalEngineerFractalEngineer
committed
AP_RangeFinder: add DTS6012M driver support
1 parent a090813 commit c234be2

6 files changed

Lines changed: 253 additions & 0 deletions

File tree

‎libraries/AP_RangeFinder/AP_RangeFinder.cpp‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,7 @@
6464
#include "AP_RangeFinder_Ainstein_LR_D1.h"
6565
#include "AP_RangeFinder_RDS02UF.h"
6666
#include "AP_RangeFinder_LightWare_GRF.h"
67+
#include "AP_RangeFinder_DTS6012M.h"
6768

6869
#include <AP_BoardConfig/AP_BoardConfig.h>
6970
#include <AP_Logger/AP_Logger.h>
@@ -628,6 +629,12 @@ __INITFUNC__ void RangeFinder::detect_instance(uint8_t instance, uint8_t& serial
628629
break;
629630
#endif // AP_RANGEFINDER_LIGHTWARE_GRF_ENABLED
630631

632+
#if AP_RANGEFINDER_DTS6012M_ENABLED
633+
case Type::DTS6012M:
634+
serial_create_fn = AP_RangeFinder_DTS6012M::create;
635+
break;
636+
#endif // AP_RANGEFINDER_DTS6012M_ENABLED
637+
631638
case Type::NONE:
632639
break;
633640
}

‎libraries/AP_RangeFinder/AP_RangeFinder.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -191,6 +191,9 @@ class RangeFinder
191191
#if AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
192192
BenewakeTFS20L = 46,
193193
#endif // AP_RANGEFINDER_BENEWAKE_TFS20L_ENABLED
194+
#if AP_RANGEFINDER_DTS6012M_ENABLED
195+
DTS6012M = 47,
196+
#endif // AP_RANGEFINDER_DTS6012M_ENABLED
194197
#if AP_RANGEFINDER_SIM_ENABLED
195198
SIM = 100,
196199
#endif

‎libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.cpp‎

Lines changed: 160 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,160 @@
1+
/*
2+
This program is free software: you can redistribute it and/or modify
3+
it under the terms of the GNU General Public License as published by
4+
the Free Software Foundation, either version 3 of the License, or
5+
(at your option) any later version.
6+
7+
This program is distributed in the hope that it will be useful,
8+
but WITHOUT ANY WARRANTY; without even the implied warranty of
9+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10+
GNU General Public License for more details.
11+
12+
You should have received a copy of the GNU General Public License
13+
along with this program. If not, see <http://www.gnu.org/licenses/>.
14+
*/
15+
16+
17+
#include "AP_RangeFinder_config.h"
18+
19+
#if AP_RANGEFINDER_DTS6012M_ENABLED
20+
21+
#include "AP_RangeFinder_DTS6012M.h"
22+
#include <AP_HAL/AP_HAL.h>
23+
#include <AP_Math/crc.h>
24+
25+
// DTS6012M protocol constants
26+
#define DTS6012M_FRAME_HEADER 0xA5
27+
#define DTS6012M_DEVICE_ID 0x03
28+
#define DTS6012M_DEVICE_TYPE 0x20
29+
#define DTS6012M_CMD_START_STREAM 0x01
30+
#define DTS6012M_HEADER_LEN 7 // header(1) + devid(1) + devtype(1) + cmd(1) + reserved(1) + length(2)
31+
#define DTS6012M_DATA_LEN 14 // measurement data length
32+
#define DTS6012M_CRC_LEN 2
33+
#define DTS6012M_FRAME_LEN (DTS6012M_HEADER_LEN + DTS6012M_DATA_LEN + DTS6012M_CRC_LEN) // 23 bytes
34+
#define DTS6012M_DIST_MAX_MM 20000 // 20m max range
35+
#define DTS6012M_DIST_INVALID 0xFFFF
36+
37+
extern const AP_HAL::HAL& hal;
38+
39+
/*
40+
send the start stream command (0x01) to begin periodic measurement output
41+
Frame: A5 03 20 01 00 00 00 CRC16_H CRC16_L
42+
*/
43+
void AP_RangeFinder_DTS6012M::send_start_command()
44+
{
45+
static const uint8_t cmd[] = {
46+
DTS6012M_FRAME_HEADER, // 0xA5
47+
DTS6012M_DEVICE_ID, // 0x03
48+
DTS6012M_DEVICE_TYPE, // 0x20
49+
DTS6012M_CMD_START_STREAM, // 0x01
50+
0x00, // reserved
51+
0x00, 0x00 // length = 0 (no data)
52+
};
53+
54+
// calculate CRC over the command bytes
55+
const uint16_t crc = calc_crc_modbus(cmd, sizeof(cmd));
56+
57+
uart->write(cmd, sizeof(cmd));
58+
// CRC is sent high byte first per protocol spec
59+
uart->write(uint8_t(crc >> 8));
60+
uart->write(uint8_t(crc & 0xFF));
61+
}
62+
63+
/*
64+
search for frame header byte in buffer starting at offset,
65+
shifting remaining data to re-sync after line noise
66+
*/
67+
void AP_RangeFinder_DTS6012M::find_signature_in_buffer(uint8_t start)
68+
{
69+
uint8_t *buf = (uint8_t *)&frame;
70+
for (uint8_t i = start; i < linebuf_len; i++) {
71+
if (buf[i] == DTS6012M_FRAME_HEADER) {
72+
memmove(&buf[0], &buf[i], linebuf_len - i);
73+
linebuf_len -= i;
74+
return;
75+
}
76+
}
77+
linebuf_len = 0;
78+
}
79+
80+
/*
81+
read from the sensor and return distance in meters
82+
see frame struct in header for protocol layout
83+
*/
84+
bool AP_RangeFinder_DTS6012M::get_reading(float &reading_m)
85+
{
86+
if (uart == nullptr) {
87+
return false;
88+
}
89+
90+
// keep sending start command until we receive a valid reading,
91+
// allowing for slow-startup or late connection of the device
92+
if (!got_reading) {
93+
send_start_command();
94+
}
95+
96+
// bulk read available bytes into buffer
97+
uint8_t *buf = (uint8_t *)&frame;
98+
const uint8_t num_read = uart->read(&buf[linebuf_len], sizeof(frame) - linebuf_len);
99+
linebuf_len += num_read;
100+
101+
if (linebuf_len == 0) {
102+
return false;
103+
}
104+
105+
// ensure buffer starts with frame header
106+
if (frame.header != DTS6012M_FRAME_HEADER) {
107+
find_signature_in_buffer(1);
108+
return false;
109+
}
110+
111+
// wait for a complete frame
112+
if (linebuf_len < DTS6012M_FRAME_LEN) {
113+
return false;
114+
}
115+
116+
// validate header fields
117+
if (frame.device_id != DTS6012M_DEVICE_ID ||
118+
frame.device_type != DTS6012M_DEVICE_TYPE ||
119+
frame.command != DTS6012M_CMD_START_STREAM) {
120+
find_signature_in_buffer(1);
121+
return false;
122+
}
123+
124+
// verify length field
125+
if (be16toh(frame.data_len) != DTS6012M_DATA_LEN) {
126+
find_signature_in_buffer(1);
127+
return false;
128+
}
129+
130+
// verify CRC-16 over header + data (all bytes except last 2)
131+
const uint16_t crc_calc = calc_crc_modbus(buf, DTS6012M_FRAME_LEN - DTS6012M_CRC_LEN);
132+
if (crc_calc != be16toh(frame.crc)) {
133+
find_signature_in_buffer(1);
134+
return false;
135+
}
136+
137+
// extract primary target distance (little-endian, in mm)
138+
const uint16_t dist_mm = le16toh(frame.primary_distance_mm);
139+
140+
// capture primary target intensity for signal quality
141+
const int32_t intensity = int32_t(le16toh(frame.primary_intensity));
142+
// map intensity to 0-100%, clamping at 10000 as practical maximum
143+
_signal_quality_pct = int8_t(constrain_int32(intensity * 100 / 10000, 0, 100));
144+
145+
// frame consumed, reset buffer and discard any stale data so the
146+
// next call reads the freshest frame from this high-rate sensor
147+
linebuf_len = 0;
148+
uart->discard_input();
149+
got_reading = true;
150+
151+
if (dist_mm == DTS6012M_DIST_INVALID || dist_mm > DTS6012M_DIST_MAX_MM) {
152+
reading_m = max_distance() + 1.0f;
153+
return true;
154+
}
155+
156+
reading_m = dist_mm * 0.001f;
157+
return true;
158+
}
159+
160+
#endif // AP_RANGEFINDER_DTS6012M_ENABLED

‎libraries/AP_RangeFinder/AP_RangeFinder_DTS6012M.h‎

Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
/*
2+
This program is free software: you can redistribute it and/or modify
3+
it under the terms of the GNU General Public License as published by
4+
the Free Software Foundation, either version 3 of the License, or
5+
(at your option) any later version.
6+
7+
This program is distributed in the hope that it will be useful,
8+
but WITHOUT ANY WARRANTY; without even the implied warranty of
9+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10+
GNU General Public License for more details.
11+
12+
You should have received a copy of the GNU General Public License
13+
along with this program. If not, see <http://www.gnu.org/licenses/>.
14+
*/
15+
16+
#pragma once
17+
18+
#include "AP_RangeFinder_config.h"
19+
20+
#if AP_RANGEFINDER_DTS6012M_ENABLED
21+
22+
#include "AP_RangeFinder_Backend_Serial.h"
23+
#include <AP_HAL/utility/sparse-endian.h>
24+
25+
26+
class AP_RangeFinder_DTS6012M : public AP_RangeFinder_Backend_Serial
27+
{
28+
public:
29+
static AP_RangeFinder_Backend_Serial *create(
30+
RangeFinder::RangeFinder_State &_state,
31+
AP_RangeFinder_Params &_params)
32+
{
33+
return NEW_NOTHROW AP_RangeFinder_DTS6012M(_state, _params);
34+
}
35+
36+
protected:
37+
using AP_RangeFinder_Backend_Serial::AP_RangeFinder_Backend_Serial;
38+
39+
MAV_DISTANCE_SENSOR _get_mav_distance_sensor_type() const override { return MAV_DISTANCE_SENSOR_LASER; }
40+
41+
private:
42+
bool get_reading(float &reading_m) override;
43+
uint32_t initial_baudrate(uint8_t serial_instance) const override { return 921600; }
44+
int8_t get_signal_quality_pct() const override { return _signal_quality_pct; }
45+
46+
// protocol parsing state
47+
// response frame layout matching the DTS6012M wire format
48+
struct PACKED {
49+
uint8_t header; // 0xA5
50+
uint8_t device_id; // 0x03
51+
uint8_t device_type; // 0x20
52+
uint8_t command; // 0x01
53+
uint8_t reserved;
54+
be16_t data_len; // big-endian length
55+
// measurement data (14 bytes)
56+
le16_t secondary_distance_mm;
57+
le16_t secondary_correction;
58+
le16_t secondary_intensity;
59+
le16_t primary_distance_mm;
60+
le16_t primary_correction;
61+
le16_t primary_intensity;
62+
le16_t sunlight_base;
63+
// CRC16 (big-endian)
64+
be16_t crc;
65+
} frame;
66+
uint8_t linebuf_len = 0;
67+
bool got_reading = false;
68+
int8_t _signal_quality_pct = RangeFinder::SIGNAL_QUALITY_UNKNOWN;
69+
70+
// send start stream command to sensor
71+
void send_start_command();
72+
73+
// search for frame header byte starting at given offset,
74+
// shifting buffer contents to re-sync after line noise
75+
void find_signature_in_buffer(uint8_t start);
76+
};
77+
78+
#endif // AP_RANGEFINDER_DTS6012M_ENABLED

‎libraries/AP_RangeFinder/AP_RangeFinder_Params.cpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -61,6 +61,7 @@ const AP_Param::GroupInfo AP_RangeFinder_Params::var_info[] = {
6161
// @Values: 44:HexsoonRadar
6262
// @Values: 45:LightWare-GRF
6363
// @Values: 46:BenewakeTFS20L
64+
// @Values: 47:DTS6012M
6465
// @Values: 100:SITL
6566
// @User: Standard
6667
AP_GROUPINFO_FLAGS("TYPE", 1, AP_RangeFinder_Params, type, 0, AP_PARAM_FLAG_ENABLE),

‎libraries/AP_RangeFinder/AP_RangeFinder_config.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -199,4 +199,8 @@
199199
#define AP_RANGEFINDER_HEXSOONRADAR_ENABLED (HAL_MAX_CAN_PROTOCOL_DRIVERS && AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED)
200200
#endif
201201

202+
#ifndef AP_RANGEFINDER_DTS6012M_ENABLED
203+
#define AP_RANGEFINDER_DTS6012M_ENABLED (AP_RANGEFINDER_BACKEND_DEFAULT_ENABLED && HAL_PROGRAM_SIZE_LIMIT_KB > 1024)
204+
#endif
205+
202206
#define AP_RANGEFINDER_NRA24_CAN_DRIVER_ENABLED (AP_RANGEFINDER_HEXSOONRADAR_ENABLED || AP_RANGEFINDER_NRA24_CAN_ENABLED)

0 commit comments

Comments
 (0)