ESPHome 2026.11.0b6
Loading...
Searching...
No Matches
ld2460.cpp
Go to the documentation of this file.
1#include "ld2460.h"
2
3#include "esphome/core/hal.h"
5
6#include <cmath>
7#include <cstring>
8#include <numbers>
9
10namespace esphome::ld2460 {
11
12ESPHOME_LOG_TAG(TAG, "ld2460");
13
14static constexpr uint8_t HEADER_FOOTER_SIZE = 4;
15// PROGMEM keeps the patterns out of RAM on the ESP8266; they are copied out for each compare
16static constexpr uint8_t CMD_FRAME_HEADER[HEADER_FOOTER_SIZE] PROGMEM = {0xFD, 0xFC, 0xFB, 0xFA};
17static constexpr uint8_t CMD_FRAME_FOOTER[HEADER_FOOTER_SIZE] PROGMEM = {0x04, 0x03, 0x02, 0x01};
18static constexpr uint8_t DATA_FRAME_HEADER[HEADER_FOOTER_SIZE] PROGMEM = {0xF4, 0xF3, 0xF2, 0xF1};
19static constexpr uint8_t DATA_FRAME_FOOTER[HEADER_FOOTER_SIZE] PROGMEM = {0xF8, 0xF7, 0xF6, 0xF5};
20
21static constexpr uint8_t CMD_REPORT_DATA = 0x04;
22static constexpr uint8_t CMD_QUERY_INSTALLATION_MODE = 0x0A;
23static constexpr uint8_t ACK_FRAME_LENGTH = 12;
24static constexpr uint8_t MAX_QUERY_ATTEMPTS = 3;
25// Header, command, length (12, little endian), payload 0x01, footer
26static constexpr uint8_t QUERY_INSTALLATION_MODE_FRAME[] PROGMEM = {
27 0xFD, 0xFC, 0xFB, 0xFA, CMD_QUERY_INSTALLATION_MODE, 0x0C, 0x00, 0x01, 0x04, 0x03, 0x02, 0x01,
28};
29
30static bool matches(const uint8_t *bytes, const uint8_t *pattern) {
31 uint8_t copy[HEADER_FOOTER_SIZE];
32 progmem_memcpy(copy, pattern, sizeof(copy));
33 return std::memcmp(bytes, copy, sizeof(copy)) == 0;
34}
35
36// Header (4), command (1), length (2, little endian); the length counts the whole frame
37static constexpr uint8_t FRAME_PREFIX_SIZE = 7;
38static uint16_t frame_length(const uint8_t *frame) { return encode_uint16(frame[6], frame[5]); }
39
40// First offset after 0 whose bytes, as far as they go, match a header; len when there is none (len >= 1)
41static uint8_t next_header_start(const uint8_t *buffer, uint8_t len) {
42 uint8_t data_header[HEADER_FOOTER_SIZE];
43 uint8_t cmd_header[HEADER_FOOTER_SIZE];
44 progmem_memcpy(data_header, DATA_FRAME_HEADER, sizeof(data_header));
45 progmem_memcpy(cmd_header, CMD_FRAME_HEADER, sizeof(cmd_header));
46 uint8_t start = 1;
47 for (; start < len; start++) {
48 const size_t n = std::min<size_t>(len - start, HEADER_FOOTER_SIZE);
49 if (std::memcmp(&buffer[start], data_header, n) == 0 || std::memcmp(&buffer[start], cmd_header, n) == 0) {
50 break;
51 }
52 }
53 return start;
54}
55
56static constexpr float RADIANS_TO_DEGREES = 180.0f / std::numbers::pi_v<float>;
57
58void LD2460Component::setup() { this->send_query_(); }
59
61 uint8_t frame[sizeof(QUERY_INSTALLATION_MODE_FRAME)];
62 progmem_memcpy(frame, QUERY_INSTALLATION_MODE_FRAME, sizeof(frame));
63 this->write_array(frame, sizeof(frame));
64}
65
66void LD2460Component::dump_config() {
67 ESP_LOGCONFIG(TAG,
68 "LD2460:\n"
69 " Installation mode: %s",
70 this->query_attempts_ != 0 ? LOG_STR_LITERAL("unknown")
71 : this->installation_mode_ == InstallationMode::INSTALLATION_MODE_TOP ? LOG_STR_LITERAL("top")
72 : LOG_STR_LITERAL("side"));
73#ifdef USE_SENSOR
74 LOG_SENSOR_WITH_DEDUP(" ", "TargetCount", this->target_count_sensor_);
75 for (const auto &t : this->target_sensors_) {
76 LOG_SENSOR_WITH_DEDUP(" ", "TargetX", t.x);
77 LOG_SENSOR_WITH_DEDUP(" ", "TargetY", t.y);
78 LOG_SENSOR_WITH_DEDUP(" ", "TargetDistance", t.distance);
79 LOG_SENSOR_WITH_DEDUP(" ", "TargetAngle", t.angle);
80 }
81#endif
82}
83
84void LD2460Component::loop() {
85 this->read_available<MAX_LINE_LENGTH>([this](uint8_t b) { this->readline_(b); });
86}
87
89 const uint8_t *data = this->buffer_data_;
90 if (!this->align_to_header_(DATA_FRAME_HEADER) || data[4] != CMD_REPORT_DATA) {
91 ESP_LOGW(TAG, "Invalid periodic data header");
92 return;
93 }
94 const uint8_t packet_len = this->buffer_pos_;
95 if (packet_len < 11 || (packet_len - 11) % 4 != 0) {
96 ESP_LOGW(TAG, "Periodic packet length %u is not a whole number of targets", packet_len);
97 return;
98 }
99
100 if (this->query_attempts_ != 0) {
101 if (this->query_attempts_ < MAX_QUERY_ATTEMPTS) {
102 // The module may have booted after us and missed the earlier query
103 this->query_attempts_++;
104 this->send_query_();
105 } else if (this->query_attempts_ == MAX_QUERY_ATTEMPTS) {
106 this->query_attempts_++;
107 ESP_LOGW(TAG, "No installation mode reply after %u queries; assuming side mount", MAX_QUERY_ATTEMPTS);
108 }
109 }
110
111#ifdef USE_SENSOR
112 uint8_t num_targets = std::min<uint8_t>((packet_len - 11) / 4, MAX_TARGETS);
113 for (uint8_t i = 0; i < MAX_TARGETS; i++) {
114 TargetSensors &t = this->target_sensors_[i];
115 if (i >= num_targets) {
116 t.last_x = INT16_MIN;
117 t.last_y = INT16_MIN;
118 SAFE_PUBLISH_SENSOR_UNKNOWN(t.x);
119 SAFE_PUBLISH_SENSOR_UNKNOWN(t.y);
120 SAFE_PUBLISH_SENSOR_UNKNOWN(t.distance);
121 SAFE_PUBLISH_SENSOR_UNKNOWN(t.angle);
122 continue;
123 }
124 const uint8_t *p = &data[7 + i * 4];
125 auto raw_x = encode_value_le<int16_t>(p);
126 auto raw_y = encode_value_le<int16_t>(&p[2]);
127 if (raw_x == t.last_x && raw_y == t.last_y) {
128 continue;
129 }
130 t.last_x = raw_x;
131 t.last_y = raw_y;
132
133 // Coordinates are reported in 0.1 m steps
134 float x = raw_x * 0.1f;
135 float y = raw_y * 0.1f;
136 SAFE_PUBLISH_SENSOR(t.x, x);
137 SAFE_PUBLISH_SENSOR(t.y, y);
138 if (t.distance.has_sensor()) {
139 SAFE_PUBLISH_SENSOR(t.distance, std::sqrt(x * x + y * y));
140 }
141 if (t.angle.has_sensor()) {
142 float angle;
144 angle = std::atan2(y, x) * RADIANS_TO_DEGREES;
145 if (angle < 0.0f) {
146 angle += 360.0f;
147 }
148 } else {
149 angle = std::atan2(x, y) * RADIANS_TO_DEGREES;
150 }
151 SAFE_PUBLISH_SENSOR(t.angle, angle);
152 }
153 }
154 SAFE_PUBLISH_SENSOR(this->target_count_sensor_, num_targets);
155#endif
156
157 this->data_callback_.call();
158}
159
160void LD2460Component::readline_(uint8_t readch) {
161 if (this->buffer_pos_ >= MAX_LINE_LENGTH) {
162 // Keep the bytes from the next offset that could start a header, so a frame already partly
163 // buffered behind stray bytes survives; it is still fully validated once its footer arrives
164 const uint8_t start = next_header_start(this->buffer_data_, this->buffer_pos_);
165 this->buffer_pos_ -= start;
166 std::memmove(this->buffer_data_, &this->buffer_data_[start], this->buffer_pos_);
167 // Header-shaped noise keeps a whole header; warning only otherwise bounds the warning rate
168 if (this->buffer_pos_ < HEADER_FOOTER_SIZE) {
169 ESP_LOGW(TAG, "Max frame length exceeded; ignoring");
170 } else {
171 ESP_LOGV(TAG, "Max frame length exceeded; kept %u bytes", this->buffer_pos_);
172 }
173 }
174 this->buffer_data_[this->buffer_pos_++] = readch;
175
176 if (this->buffer_pos_ < HEADER_FOOTER_SIZE) {
177 return;
178 }
179 const uint8_t *tail = &this->buffer_data_[this->buffer_pos_ - HEADER_FOOTER_SIZE];
180 bool is_data = readch == progmem_read_byte(&DATA_FRAME_FOOTER[3]) && matches(tail, DATA_FRAME_FOOTER);
181 if (!is_data && (readch != progmem_read_byte(&CMD_FRAME_FOOTER[3]) || !matches(tail, CMD_FRAME_FOOTER))) {
182 return;
183 }
184
185 ESP_LOGV_HEX(TAG, this->buffer_data_, this->buffer_pos_, MAX_LINE_LENGTH, "Frame: %s");
186
187 if (is_data) {
188 this->handle_periodic_data_();
189 } else {
190 this->handle_ack_();
191 }
192 this->buffer_pos_ = 0;
193}
194
195// Once a footer is matched, shift the buffer to the first offset holding a header whose length
196// field ends the frame exactly at the footer. Stray bytes, truncated frames and header bytes inside
197// a payload are skipped this way. Returns false when no such offset exists.
198bool LD2460Component::align_to_header_(const uint8_t *header) {
199 for (uint8_t i = 0; i + FRAME_PREFIX_SIZE <= this->buffer_pos_; i++) {
200 const uint8_t *frame = &this->buffer_data_[i];
201 if (matches(frame, header) && frame_length(frame) == this->buffer_pos_ - i) {
202 if (i != 0) {
203 this->buffer_pos_ -= i;
204 std::memmove(this->buffer_data_, frame, this->buffer_pos_);
205 }
206 return true;
207 }
208 }
209 return false;
210}
211
213 const uint8_t *data = this->buffer_data_;
214 if (!this->align_to_header_(CMD_FRAME_HEADER) || this->buffer_pos_ != ACK_FRAME_LENGTH) {
215 ESP_LOGW(TAG, "Dropping malformed command frame");
216 return;
217 }
218 if (data[4] != CMD_QUERY_INSTALLATION_MODE) {
219 ESP_LOGD(TAG, "Ignoring reply to command 0x%02X", data[4]);
220 return;
221 }
222 const uint8_t mode = data[7];
223 if (mode != static_cast<uint8_t>(InstallationMode::INSTALLATION_MODE_SIDE) &&
224 mode != static_cast<uint8_t>(InstallationMode::INSTALLATION_MODE_TOP)) {
225 ESP_LOGW(TAG, "Unexpected installation mode %u", mode);
226 return;
227 }
228 this->installation_mode_ = static_cast<InstallationMode>(mode);
229 this->query_attempts_ = 0;
230 ESP_LOGD(TAG, "Installation mode: %s",
231 mode == static_cast<uint8_t>(InstallationMode::INSTALLATION_MODE_TOP) ? LOG_STR_LITERAL("top")
232 : LOG_STR_LITERAL("side"));
233#ifdef USE_SENSOR
234 // Angles depend on the mode, so republish every target on the next frame
235 for (auto &t : this->target_sensors_) {
236 t.last_x = INT16_MIN;
237 }
238#endif
239}
240
241} // namespace esphome::ld2460
BedjetMode mode
BedJet operating mode.
virtual void setup()
Where the component's initialization should happen.
Definition component.cpp:84
InstallationMode installation_mode_
Definition ld2460.h:62
uint8_t buffer_data_[MAX_LINE_LENGTH]
Definition ld2460.h:59
void readline_(uint8_t readch)
Definition ld2460.cpp:160
bool align_to_header_(const uint8_t *header)
Definition ld2460.cpp:198
std::array< TargetSensors, MAX_TARGETS > target_sensors_
Definition ld2460.h:55
LazyCallbackManager< void()> data_callback_
Definition ld2460.h:58
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
uint32_t buffer
Definition gif.cpp:50
constexpr float BME680_GAS_LOOKUP_TABLE_1[16] PROGMEM
Definition bme680.cpp:28
ESPHOME_LOG_TAG(TAG, "ld2460")
void progmem_memcpy(void *dst, const void *src, size_t len)
Definition hal.h:56
const void size_t len
Definition hal.h:68
constexpr T encode_value_le(const uint8_t *bytes)
Encode a value from its constituent bytes (from least to most significant) in an array with length si...
Definition helpers.h:1084
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
uint8_t progmem_read_byte(const uint8_t *addr)
Definition hal.h:51
uint16_t x
Definition tt21100.cpp:5
uint16_t y
Definition tt21100.cpp:6