14static constexpr uint8_t HEADER_FOOTER_SIZE = 4;
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};
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;
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,
30static bool matches(
const uint8_t *bytes,
const uint8_t *pattern) {
31 uint8_t copy[HEADER_FOOTER_SIZE];
33 return std::memcmp(bytes, copy,
sizeof(copy)) == 0;
37static constexpr uint8_t FRAME_PREFIX_SIZE = 7;
38static uint16_t frame_length(
const uint8_t *frame) {
return encode_uint16(frame[6], frame[5]); }
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));
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) {
56static constexpr float RADIANS_TO_DEGREES = 180.0f / std::numbers::pi_v<float>;
61 uint8_t frame[
sizeof(QUERY_INSTALLATION_MODE_FRAME)];
62 progmem_memcpy(frame, QUERY_INSTALLATION_MODE_FRAME,
sizeof(frame));
66void LD2460Component::dump_config() {
69 " Installation mode: %s",
72 : LOG_STR_LITERAL(
"side"));
74 LOG_SENSOR_WITH_DEDUP(
" ",
"TargetCount", this->target_count_sensor_);
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);
84void LD2460Component::loop() {
90 if (!this->
align_to_header_(DATA_FRAME_HEADER) || data[4] != CMD_REPORT_DATA) {
91 ESP_LOGW(TAG,
"Invalid periodic data header");
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);
107 ESP_LOGW(TAG,
"No installation mode reply after %u queries; assuming side mount", MAX_QUERY_ATTEMPTS);
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++) {
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);
124 const uint8_t *p = &data[7 + i * 4];
127 if (raw_x == t.last_x && raw_y == t.last_y) {
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));
141 if (t.angle.has_sensor()) {
144 angle = std::atan2(
y,
x) * RADIANS_TO_DEGREES;
149 angle = std::atan2(
x,
y) * RADIANS_TO_DEGREES;
151 SAFE_PUBLISH_SENSOR(t.angle, angle);
154 SAFE_PUBLISH_SENSOR(this->target_count_sensor_, num_targets);
169 ESP_LOGW(TAG,
"Max frame length exceeded; ignoring");
171 ESP_LOGV(TAG,
"Max frame length exceeded; kept %u bytes", this->
buffer_pos_);
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))) {
199 for (uint8_t i = 0; i + FRAME_PREFIX_SIZE <= this->
buffer_pos_; i++) {
201 if (matches(frame, header) && frame_length(frame) == this->
buffer_pos_ - i) {
215 ESP_LOGW(TAG,
"Dropping malformed command frame");
218 if (data[4] != CMD_QUERY_INSTALLATION_MODE) {
219 ESP_LOGD(TAG,
"Ignoring reply to command 0x%02X", data[4]);
222 const uint8_t
mode = data[7];
225 ESP_LOGW(TAG,
"Unexpected installation mode %u",
mode);
230 ESP_LOGD(TAG,
"Installation mode: %s",
232 : LOG_STR_LITERAL(
"side"));
236 t.last_x = INT16_MIN;