Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 23 additions & 1 deletion components/ld2451/frame_parser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ namespace esphome::ld2451 {
namespace {
static constexpr uint8_t DATA_HEADER[] = {0xF4, 0xF3, 0xF2, 0xF1};
static constexpr uint8_t DATA_TAIL[] = {0xF8, 0xF7, 0xF6, 0xF5};
static constexpr uint8_t CONFIG_HEADER[] = {0xFD, 0xFC, 0xFB, 0xFA};

bool parse_target_block(const std::vector<uint8_t> &payload, size_t offset, ParsedTarget &target) {
if (offset + 5 > payload.size()) {
Expand Down Expand Up @@ -75,6 +76,27 @@ bool FrameParser::pop(ParsedFrame &frame) {
}

if (header_pos == this->buffer_.size()) {
// No data header found. Check if there's a config/ACK frame we can skip wholesale.
for (size_t i = 0; i + 4 <= this->buffer_.size(); i++) {
if (this->buffer_[i] == CONFIG_HEADER[0] && this->buffer_[i + 1] == CONFIG_HEADER[1] &&
this->buffer_[i + 2] == CONFIG_HEADER[2] && this->buffer_[i + 3] == CONFIG_HEADER[3]) {
// Need at least 6 bytes (header + length) to read the payload length.
if (i + 6 > this->buffer_.size()) {
break;
}
const uint16_t cfg_payload_len =
static_cast<uint16_t>(this->buffer_[i + 4]) | (static_cast<uint16_t>(this->buffer_[i + 5]) << 8);
const size_t cfg_frame_len = static_cast<size_t>(cfg_payload_len) + 10;
if (i + cfg_frame_len > this->buffer_.size()) {
// Incomplete config frame — wait for more data.
break;
}
// Discard everything up to and including the complete config frame.
this->buffer_.erase(this->buffer_.begin(), this->buffer_.begin() + static_cast<long>(i + cfg_frame_len));
return false;
}
}

if (this->buffer_.size() > 3) {
this->buffer_.erase(this->buffer_.begin(), this->buffer_.end() - 3);
}
Expand All @@ -99,7 +121,7 @@ bool FrameParser::pop(ParsedFrame &frame) {
if (this->buffer_[tail_pos] != DATA_TAIL[0] || this->buffer_[tail_pos + 1] != DATA_TAIL[1] ||
this->buffer_[tail_pos + 2] != DATA_TAIL[2] || this->buffer_[tail_pos + 3] != DATA_TAIL[3]) {
this->buffer_.erase(this->buffer_.begin());
return true;
return false; // Fix 1: tail mismatch is not a successful pop
}

std::vector<uint8_t> payload;
Expand Down
45 changes: 40 additions & 5 deletions components/ld2451/ld2451.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,8 @@ static const char *const TAG = "ld2451";

static const uint8_t DATA_HEADER[] = {0xF4, 0xF3, 0xF2, 0xF1};
static const uint8_t DATA_TAIL[] = {0xF8, 0xF7, 0xF6, 0xF5};
static const uint8_t CONFIG_HEADER[] = {0xFD, 0xFC, 0xFB, 0xFA};
static const uint8_t CONFIG_TAIL[] = {0x04, 0x03, 0x02, 0x01};

void LD2451Component::set_live_target_angle_sensor(uint8_t slot, sensor::Sensor *sensor) {
if (slot < kLiveTargetSlotCount) {
Expand Down Expand Up @@ -79,11 +81,14 @@ void LD2451Component::loop() {
bytes_read++;
}

bool parsed_frame = false;
while (this->extract_frame_()) {
parsed_frame = true;
bool any_frame = false;
bool keep_going = true;
while (keep_going) {
bool frame_produced = false;
keep_going = this->extract_frame_(frame_produced);
any_frame |= frame_produced;
}
if (bytes_read > 0 && !parsed_frame) {
if (bytes_read > 0 && !any_frame) {
if (now - this->last_rx_activity_log_ms_ > 5000) {
ESP_LOGD(TAG, "RX activity: read=%u bytes, buffered=%u bytes", static_cast<unsigned int>(bytes_read),
static_cast<unsigned int>(this->rx_buffer_.size()));
Expand Down Expand Up @@ -123,7 +128,7 @@ void LD2451Component::dump_config() {

float LD2451Component::get_setup_priority() const { return setup_priority::DATA; }

bool LD2451Component::extract_frame_() {
bool LD2451Component::extract_frame_(bool &frame_produced) {
if (this->rx_buffer_.size() < 10) {
return false;
}
Expand All @@ -138,6 +143,35 @@ bool LD2451Component::extract_frame_() {
}

if (header_pos == this->rx_buffer_.size()) {
// No data header found. Check for a config/ACK frame and skip it cleanly.
size_t config_pos = this->rx_buffer_.size();
for (size_t i = 0; i + 4 <= this->rx_buffer_.size(); i++) {
if (this->rx_buffer_[i] == CONFIG_HEADER[0] && this->rx_buffer_[i + 1] == CONFIG_HEADER[1] &&
this->rx_buffer_[i + 2] == CONFIG_HEADER[2] && this->rx_buffer_[i + 3] == CONFIG_HEADER[3]) {
config_pos = i;
break;
}
}
if (config_pos < this->rx_buffer_.size()) {
// Found a config header. Need at least 10 bytes (header + len + tail) to read the length.
const size_t bytes_from_header = this->rx_buffer_.size() - config_pos;
if (bytes_from_header >= 10) {
const uint16_t payload_len = static_cast<uint16_t>(this->rx_buffer_[config_pos + 4]) |
(static_cast<uint16_t>(this->rx_buffer_[config_pos + 5]) << 8);
const size_t frame_len = static_cast<size_t>(payload_len) + 10;
if (bytes_from_header >= frame_len) {
ESP_LOGD(TAG, "Config/ACK frame skipped (payload_len=%u)", payload_len);
this->rx_buffer_.erase(this->rx_buffer_.begin(),
this->rx_buffer_.begin() + static_cast<long>(config_pos + frame_len));
return true;
}
// Not enough bytes yet for the full config frame; leave the buffer untouched.
return false;
}
// Not enough bytes yet to read the length; leave the buffer untouched.
return false;
}
// No config header either — discard all but the last 3 bytes (may be a partial header).
if (this->rx_buffer_.size() > 3) {
this->rx_buffer_.erase(this->rx_buffer_.begin(), this->rx_buffer_.end() - 3);
}
Expand Down Expand Up @@ -199,6 +233,7 @@ bool LD2451Component::extract_frame_() {
this->publish_frame_(target_count, targets, alarm, has_targets);

this->rx_buffer_.erase(this->rx_buffer_.begin(), this->rx_buffer_.begin() + static_cast<long>(frame_len));
frame_produced = true;
return true;
}

Expand Down
2 changes: 1 addition & 1 deletion components/ld2451/ld2451.h
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class LD2451Component : public Component, public uart::UARTDevice {
void set_live_target_direction_text_sensor(uint8_t slot, text_sensor::TextSensor *sensor);

protected:
bool extract_frame_();
bool extract_frame_(bool &frame_produced);
bool parse_payload_(const std::vector<uint8_t> &payload, uint8_t &target_count, bool &alarm,
std::vector<ParsedTarget> &targets);
void publish_frame_(uint8_t target_count, const std::vector<ParsedTarget> &targets, bool alarm, bool has_targets);
Expand Down
38 changes: 38 additions & 0 deletions components/ld2451/tests_host/frame_parser_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -64,5 +64,43 @@ int main() {
assert(!empty_frame.has_target);
assert(empty_frame.target_count == 0);


// Test A: config/ACK frame (FD FC FB FA ... 04 03 02 01) is skipped cleanly.
// Minimal config frame: header(4) + len(2=0x02,0x00) + payload(2: AA BB) + tail(4) = 12 bytes.
FrameParser cfg_skip_parser;
std::vector<uint8_t> cfg_frame = {
0xFD, 0xFC, 0xFB, 0xFA, // config header
0x02, 0x00, // payload length = 2
0xAA, 0xBB, // payload
0x04, 0x03, 0x02, 0x01, // config tail
};
cfg_skip_parser.push(cfg_frame.data(), cfg_frame.size());
ParsedFrame cfg_frame_out{};
assert(!cfg_skip_parser.pop(cfg_frame_out)); // no data frame popped
// Buffer should be empty after skipping the config frame.
// Verify by pushing a real data frame and confirming it parses.
std::vector<uint8_t> after_cfg_data = {
0xF4, 0xF3, 0xF2, 0xF1, 0x07, 0x00, 0x01, 0x00, 0x8A,
0x10, 0x01, 0x14, 0x22, 0xF8, 0xF7, 0xF6, 0xF5,
};
cfg_skip_parser.push(after_cfg_data.data(), after_cfg_data.size());
ParsedFrame after_cfg_frame{};
assert(cfg_skip_parser.pop(after_cfg_frame));
assert(after_cfg_frame.has_target);
assert(after_cfg_frame.first_target.distance == 0x10);

// Test B: tail mismatch returns false, not true.
// Build a buffer that has the data header but wrong tail bytes.
FrameParser tail_mismatch_parser;
std::vector<uint8_t> bad_tail_bytes = {
0xF4, 0xF3, 0xF2, 0xF1, // data header
0x07, 0x00, // payload length = 7
0x01, 0x01, 0x8A, 0x10, 0x01, 0x14, 0x22, // payload
0xFF, 0xFF, 0xFF, 0xFF, // wrong tail (should be F8 F7 F6 F5)
};
tail_mismatch_parser.push(bad_tail_bytes.data(), bad_tail_bytes.size());
ParsedFrame tail_mismatch_frame{};
assert(!tail_mismatch_parser.pop(tail_mismatch_frame)); // must be false

return 0;
}
4 changes: 4 additions & 0 deletions components/ld2451/types.h
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,10 @@ struct SensorSettings {
struct ParsedTarget {
int angle{0};
uint8_t distance{0};
// 0x00 = Approaching (towards sensor), 0x01 = Moving away.
// jbeale monitor and Fiooodooor LD245X library agree on this encoding.
// Madproforg's notes flag ambiguity in the official PDF; the
// example code and third-party implementations use 0 = towards.
uint8_t direction{0};
uint8_t speed{0};
uint8_t snr{0};
Expand Down