ESPHome 2026.11.0b6
Loading...
Searching...
No Matches
rd03d.cpp
Go to the documentation of this file.
1#include "rd03d.h"
3#include "esphome/core/log.h"
4
5#include <cinttypes>
6#include <cmath>
7#include <numbers>
8
9namespace esphome::rd03d {
10
11ESPHOME_LOG_TAG(TAG, "rd03d");
12
13// Delay before sending configuration commands to allow radar to initialize
14static constexpr uint32_t SETUP_TIMEOUT_MS = 100;
15
16// Data frame format (radar -> host)
17static constexpr uint8_t FRAME_HEADER[] = {0xAA, 0xFF, 0x03, 0x00};
18static constexpr uint8_t FRAME_FOOTER[] = {0x55, 0xCC};
19
20// Command frame format (host -> radar)
21static constexpr uint8_t CMD_FRAME_HEADER[] = {0xFD, 0xFC, 0xFB, 0xFA};
22static constexpr uint8_t CMD_FRAME_FOOTER[] = {0x04, 0x03, 0x02, 0x01};
23
24// RD-03D tracking mode commands
25static constexpr uint16_t CMD_SINGLE_TARGET = 0x0080;
26static constexpr uint16_t CMD_MULTI_TARGET = 0x0090;
27
28// Speed sentinel values (cm/s) - radar outputs these when no valid Doppler measurement
29// FMCW radars detect motion via Doppler shift; targets with these speeds are likely noise
30static constexpr int16_t SPEED_SENTINEL_248 = 248;
31static constexpr int16_t SPEED_SENTINEL_256 = 256;
32
33// Decode coordinate/speed value from RD-03D format
34// Per datasheet: MSB=1 means positive, MSB=0 means negative
35static constexpr int16_t decode_value(uint8_t low_byte, uint8_t high_byte) {
36 int16_t value = encode_uint16(high_byte & 0x7F, low_byte);
37 if ((high_byte & 0x80) == 0) {
38 value = -value;
39 }
40 return value;
41}
42
43// Check if speed value indicates a valid Doppler measurement
44// Zero, ±248, or ±256 cm/s are sentinel values from the radar firmware
45static constexpr bool is_speed_valid(int16_t speed) {
46 int16_t abs_speed = speed < 0 ? -speed : speed;
47 return speed != 0 && abs_speed != SPEED_SENTINEL_248 && abs_speed != SPEED_SENTINEL_256;
48}
49
51 ESP_LOGCONFIG(TAG, "Setting up RD-03D...");
52 this->set_timeout(SETUP_TIMEOUT_MS, [this]() { this->apply_config_(); });
53}
54
55void RD03DComponent::dump_config() {
56 ESP_LOGCONFIG(TAG, "RD-03D:");
57 if (this->tracking_mode_.has_value()) {
58 ESP_LOGCONFIG(
59 TAG, " Tracking Mode: %s",
60 *this->tracking_mode_ == TrackingMode::SINGLE_TARGET ? LOG_STR_LITERAL("single") : LOG_STR_LITERAL("multi"));
61 }
62 if (this->throttle_ > 0) {
63 ESP_LOGCONFIG(TAG, " Throttle: %" PRIu32 "ms", this->throttle_);
64 }
65#ifdef USE_SENSOR
66 LOG_SENSOR(" ", "Target Count", this->target_count_sensor_);
67#endif
68#ifdef USE_BINARY_SENSOR
69 LOG_BINARY_SENSOR(" ", "Target", this->target_binary_sensor_);
70#endif
71 for (uint8_t i = 0; i < MAX_TARGETS; i++) {
72 ESP_LOGCONFIG(TAG, " Target %d:", i + 1);
73#ifdef USE_SENSOR
74 LOG_SENSOR(" ", "X", this->targets_[i].x);
75 LOG_SENSOR(" ", "Y", this->targets_[i].y);
76 LOG_SENSOR(" ", "Speed", this->targets_[i].speed);
77 LOG_SENSOR(" ", "Distance", this->targets_[i].distance);
78 LOG_SENSOR(" ", "Resolution", this->targets_[i].resolution);
79 LOG_SENSOR(" ", "Angle", this->targets_[i].angle);
80#endif
81#ifdef USE_BINARY_SENSOR
82 LOG_BINARY_SENSOR(" ", "Presence", this->target_presence_[i]);
83#endif
84 }
85}
86
87void RD03DComponent::loop() {
88 this->read_available([this](uint8_t byte) {
89 ESP_LOGVV(TAG, "Received byte: 0x%02X, buffer_pos: %d", byte, this->buffer_pos_);
90
91 // Check if we're looking for frame header
92 if (this->buffer_pos_ < FRAME_HEADER_SIZE) {
93 if (byte == FRAME_HEADER[this->buffer_pos_]) {
94 this->buffer_[this->buffer_pos_++] = byte;
95 } else if (byte == FRAME_HEADER[0]) {
96 // Start over if we see a potential new header
97 this->buffer_[0] = byte;
98 this->buffer_pos_ = 1;
99 } else {
100 this->buffer_pos_ = 0;
101 }
102 return;
103 }
104
105 // Accumulate data bytes
106 this->buffer_[this->buffer_pos_++] = byte;
107
108 // Check if we have a complete frame
109 if (this->buffer_pos_ == FRAME_SIZE) {
110 // Validate footer
111 if (this->buffer_[FRAME_SIZE - 2] == FRAME_FOOTER[0] && this->buffer_[FRAME_SIZE - 1] == FRAME_FOOTER[1]) {
112 this->process_frame_();
113 } else {
114 ESP_LOGW(TAG, "Invalid frame footer: 0x%02X 0x%02X (expected 0x55 0xCC)", this->buffer_[FRAME_SIZE - 2],
115 this->buffer_[FRAME_SIZE - 1]);
116 }
117 this->buffer_pos_ = 0;
118 }
119 });
120}
121
123 // Apply throttle if configured
124 if (this->throttle_ > 0) {
125 uint32_t now = millis();
126 if (now - this->last_publish_time_ < this->throttle_) {
127 return;
128 }
129 this->last_publish_time_ = now;
130 }
131
132 uint8_t target_count = 0;
133
134 for (uint8_t i = 0; i < MAX_TARGETS; i++) {
135 // Calculate offset for this target's data
136 // Header is 4 bytes, each target is 8 bytes
137 uint8_t offset = FRAME_HEADER_SIZE + (i * TARGET_DATA_SIZE);
138
139 // Extract raw bytes for this target (per datasheet Table 5-2: X, Y, Speed, Resolution)
140 uint8_t x_low = this->buffer_[offset + 0];
141 uint8_t x_high = this->buffer_[offset + 1];
142 uint8_t y_low = this->buffer_[offset + 2];
143 uint8_t y_high = this->buffer_[offset + 3];
144 uint8_t speed_low = this->buffer_[offset + 4];
145 uint8_t speed_high = this->buffer_[offset + 5];
146 uint8_t res_low = this->buffer_[offset + 6];
147 uint8_t res_high = this->buffer_[offset + 7];
148
149 // Decode values per RD-03D format
150 int16_t x = decode_value(x_low, x_high);
151 int16_t y = decode_value(y_low, y_high);
152 int16_t speed = decode_value(speed_low, speed_high);
153 uint16_t resolution = encode_uint16(res_high, res_low);
154
155 // Check if target is present
156 // Requires non-zero coordinates AND valid speed (not a sentinel value)
157 // FMCW radars detect motion via Doppler; sentinel speed indicates no real target
158 bool has_position = (x != 0 || y != 0);
159 bool has_valid_speed = is_speed_valid(speed);
160 bool target_present = has_position && has_valid_speed;
161 if (target_present) {
162 target_count++;
163 }
164
165#ifdef USE_SENSOR
166 this->publish_target_(i, x, y, speed, resolution);
167#endif
168
169#ifdef USE_BINARY_SENSOR
170 PUBLISH_STATE(this->target_presence_[i], target_present);
171#endif
172 }
173
174#ifdef USE_SENSOR
175 PUBLISH_STATE(this->target_count_sensor_, target_count);
176#endif
177
178#ifdef USE_BINARY_SENSOR
179 PUBLISH_STATE(this->target_binary_sensor_, target_count > 0);
180#endif
181}
182
183#ifdef USE_SENSOR
184void RD03DComponent::publish_target_(uint8_t target_num, int16_t x, int16_t y, int16_t speed, uint16_t resolution) {
185 TargetSensor &target = this->targets_[target_num];
186 bool valid = is_speed_valid(speed);
187
188 // Publish X coordinate (mm) - NaN if target invalid
189 PUBLISH_STATE(target.x, valid ? static_cast<float>(x) : NAN);
190
191 // Publish Y coordinate (mm) - NaN if target invalid
192 PUBLISH_STATE(target.y, valid ? static_cast<float>(y) : NAN);
193
194 // Publish speed (convert from cm/s to mm/s) - NaN if target invalid
195 PUBLISH_STATE(target.speed, valid ? static_cast<float>(speed) * 10.0f : NAN);
196
197 // Publish resolution (mm)
198 PUBLISH_STATE(target.resolution, resolution);
199
200 // Calculate and publish distance (mm) - NaN if target invalid
201 if (target.distance != nullptr) {
202 if (valid) {
203 target.distance->publish_state(std::hypot(static_cast<float>(x), static_cast<float>(y)));
204 } else {
205 target.distance->publish_state(NAN);
206 }
207 }
208
209 // Calculate and publish angle (degrees) - NaN if target invalid
210 // Angle is measured from the Y axis (radar forward direction)
211 if (target.angle != nullptr) {
212 if (valid) {
213 float angle = std::atan2(static_cast<float>(x), static_cast<float>(y)) * 180.0f / std::numbers::pi_v<float>;
214 target.angle->publish_state(angle);
215 } else {
216 target.angle->publish_state(NAN);
217 }
218 }
219}
220#endif
221
222void RD03DComponent::send_command_(uint16_t command, const uint8_t *data, uint8_t data_len) {
223 // Send header
224 this->write_array(CMD_FRAME_HEADER, sizeof(CMD_FRAME_HEADER));
225
226 // Send length (command word + data)
227 uint16_t len = 2 + data_len;
228 this->write_byte(len & 0xFF);
229 this->write_byte((len >> 8) & 0xFF);
230
231 // Send command word (little-endian)
232 this->write_byte(command & 0xFF);
233 this->write_byte((command >> 8) & 0xFF);
234
235 // Send data if any
236 if (data != nullptr && data_len > 0) {
237 this->write_array(data, data_len);
238 }
239
240 // Send footer
241 this->write_array(CMD_FRAME_FOOTER, sizeof(CMD_FRAME_FOOTER));
242
243 ESP_LOGD(TAG, "Sent command 0x%04X with %d bytes of data", command, data_len);
244}
245
247 if (this->tracking_mode_.has_value()) {
248 uint16_t mode_cmd = (*this->tracking_mode_ == TrackingMode::SINGLE_TARGET) ? CMD_SINGLE_TARGET : CMD_MULTI_TARGET;
249 this->send_command_(mode_cmd);
250 }
251}
252
253} // namespace esphome::rd03d
virtual void setup()
Where the component's initialization should happen.
Definition component.cpp:84
void set_timeout(const char *name, uint32_t timeout, std::function< void()> &&f)
Set a timeout function with a const char* name.
Definition component.cpp:96
optional< TrackingMode > tracking_mode_
Definition rd03d.h:87
std::array< TargetSensor, MAX_TARGETS > targets_
Definition rd03d.h:80
void send_command_(uint16_t command, const uint8_t *data=nullptr, uint8_t data_len=0)
Definition rd03d.cpp:222
std::array< uint8_t, FRAME_SIZE > buffer_
Definition rd03d.h:91
void publish_target_(uint8_t target_num, int16_t x, int16_t y, int16_t speed, uint16_t resolution)
Definition rd03d.cpp:184
std::array< binary_sensor::BinarySensor *, MAX_TARGETS > target_presence_
Definition rd03d.h:83
void publish_state(float state)
Publish a new state to the front-end.
Definition sensor.cpp:65
void write_byte(uint8_t data)
Definition uart.h:22
bool read_available(F &&on_byte)
Same as read_available_chunks(), calling on_byte(uint8_t) once per byte.
Definition uart.h:77
void write_array(const uint8_t *data, size_t len)
Definition uart.h:30
int speed
Definition fan.h:3
Resolution resolution
Definition msa3xx.h:1
ESPHOME_LOG_TAG(TAG, "rd03d")
uint32_t value
Definition hal.cpp:41
const void size_t len
Definition hal.h:68
constexpr uint16_t encode_uint16(uint8_t msb, uint8_t lsb)
Encode a 16-bit value given the most and least significant byte.
Definition helpers.h:1043
uint32_t IRAM_ATTR HOT millis()
Definition hal.cpp:25
bool valid
static void uint32_t
sensor::Sensor * resolution
Definition rd03d.h:35
sensor::Sensor * distance
Definition rd03d.h:34
sensor::Sensor * x
Definition rd03d.h:31
sensor::Sensor * speed
Definition rd03d.h:33
sensor::Sensor * y
Definition rd03d.h:32
sensor::Sensor * angle
Definition rd03d.h:36
uint16_t x
Definition tt21100.cpp:5
uint16_t y
Definition tt21100.cpp:6