orangepivietnam/esp32s3-ld2453
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update 3 targets

Browse Source
This commit is contained in:
admin committed 2026-09-14 16:46:16 +07:00
1 parent 04da39494d
commit ab6ae71014
3 files changed
+103 -60
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

esp32s3-ld2453/LD2453Parser.cpp
+98 -57
View File
@@ -83,10 +83,24 @@ static const size_t TAIL_SIZE = 2;
static const size_t TARGET_RECORD_SIZE = 8;
static const size_t MAX_TARGETS = 3;
/* LD2453 / LD2450 0x0003 payload layout (variable length!):
* [0] Group byte (usually 0x00 for the default tracking group)
* [1] Count byte (0..3 — how many 8-byte target records follow)
* [2..2+N*8-1] Target records, N = Count byte.
* Total frame length = HEADER(4) + 2(group+count) + N*8 + TAIL(2)
* = 8 + N * 8 (8, 16, 24, or 32 bytes for N=0..3)
*
* NOTE: Previously we hard-coded 30-byte frames (always 3 targets). That
* was the ROOT CAUSE of "always 1 target": when the radar actually tracked
* 2 people, Count=2 and it only sent 2*8 record bytes. The fixed parser
* then read slot#2 bytes starting from the FRAME_TAIL bytes (0x55 0xCC ...)
* → sign-magnitude decode produced a 20000+ mm coordinate → caught by the
* plausibility filter → valid=false → "only 1 target ever" in the UI.
*/
static const size_t PAYLOAD_HEADER_OVERHEAD = 2; /* Group + Count bytes */
static const size_t MIN_TARGET_REPORT_LEN = HEADER_SIZE + PAYLOAD_HEADER_OVERHEAD + 0 * TARGET_RECORD_SIZE + TAIL_SIZE; /* 8 bytes */
static const size_t MAX_TARGET_REPORT_LEN = HEADER_SIZE + PAYLOAD_HEADER_OVERHEAD + MAX_TARGETS * TARGET_RECORD_SIZE + TAIL_SIZE; /* 32 bytes */
static const uint16_t FRAME_TYPE_TARGETS_LD2450 = 0x0003u;
static const size_t TARGET_REPORT_TOTAL_LEN = 30;
static const size_t TARGET_REPORT_PAYLOAD_LEN =
TARGET_REPORT_TOTAL_LEN - HEADER_SIZE - TAIL_SIZE; // 24 bytes
static const uint16_t accepted_target_types[] = {
FRAME_TYPE_TARGETS_LD2450,
@@ -205,22 +219,22 @@ bool LD2453Parser::update() {
}
bool LD2453Parser::tryParseFrame() {
if (_bufferLen < TARGET_REPORT_TOTAL_LEN) return false;
if (_bufferLen < MIN_TARGET_REPORT_LEN) return false;
// ----------------------------------------------------------------
// STEP 1 – Locate [0xAA 0xFF] sync, decode the uint16 LE frame-type
// that follows, keep only types we know how to interpret today.
// Per user request, KNOWN frames (type 0x0003) will ALL be promoted
// to the live RadarState — filtering is per-slot WARN below, never
// whole-frame drop, so we never accidentally lose a real target.
// STEP 1 – Locate [0xAA 0xFF] sync, decode the uint16 LE frame-type,
// then read the Group + Count bytes to know how many target records
// actually follow (the radar sends VARIABLE-length frames, NOT an
// always-3-slot fixed 24-byte payload).
// ----------------------------------------------------------------
size_t headerPos = 0;
bool foundSync = false;
uint16_t frameType = 0u;
size_t frameLen = TARGET_REPORT_TOTAL_LEN;
size_t payloadLen = TARGET_REPORT_PAYLOAD_LEN;
size_t headerPos = 0;
bool foundSync = false;
uint16_t frameType = 0u;
size_t frameLen = 0u;
size_t payloadLen = 0u;
uint8_t reportedCount = 0u;
for (size_t i = 0; i <= _bufferLen - TARGET_REPORT_TOTAL_LEN; i++) {
for (size_t i = 0; i <= _bufferLen - MIN_TARGET_REPORT_LEN; i++) {
if (memcmp(_ringBuffer + i, FRAME_SYNC, SYNC_SIZE) != 0) continue;
const uint16_t candidateType =
@@ -237,9 +251,30 @@ bool LD2453Parser::tryParseFrame() {
return tryParseFrame();
}
if ((i + HEADER_SIZE + PAYLOAD_HEADER_OVERHEAD) > _bufferLen) {
return false; // wait for more bytes (Group + Count not yet received)
}
const uint8_t rawGroup = _ringBuffer[i + HEADER_SIZE + 0];
const uint8_t rawCount = _ringBuffer[i + HEADER_SIZE + 1];
if (rawCount > MAX_TARGETS) {
// Malformed/unsupported count — skip 1 byte past sync and retry
memmove(_ringBuffer, _ringBuffer + i + 1, _bufferLen - i - 1);
_bufferLen -= (i + 1);
return tryParseFrame();
}
reportedCount = rawCount;
payloadLen = PAYLOAD_HEADER_OVERHEAD + (size_t)reportedCount * TARGET_RECORD_SIZE;
frameLen = HEADER_SIZE + payloadLen + TAIL_SIZE;
if (frameLen > MAX_TARGET_REPORT_LEN) {
memmove(_ringBuffer, _ringBuffer + i + 1, _bufferLen - i - 1);
_bufferLen -= (i + 1);
return tryParseFrame();
}
headerPos = i;
foundSync = true;
frameType = candidateType;
(void)rawGroup;
break;
}
@@ -263,11 +298,16 @@ bool LD2453Parser::tryParseFrame() {
// ----------------------------------------------------------------
// STEP 2 – Parse slots directly into the live _lastFrame.
// User policy: ALWAYS accept/promote the frame (increase counters
// and bump sequence). We only filter at the per-slot level via
// the WARN rule below.
// * Slots 0 .. reportedCount-1 come from the real payload bytes
// starting at offset PAYLOAD_HEADER_OVERHEAD (after Group+Count).
// * Slots reportedCount .. MAX_TARGETS-1 are force-marked EMPTY
// (valid=false x=y=speed=0) because the radar did not emit them
// for this frame.
// User policy: NEVER reject the whole 0x0003 frame — promote counters
// always. Filtering is strictly per-slot WARN below.
// ----------------------------------------------------------------
uint8_t *data = _ringBuffer + headerPos + HEADER_SIZE;
uint8_t *data = _ringBuffer + headerPos + HEADER_SIZE;
uint8_t *slotAreaStart = data + PAYLOAD_HEADER_OVERHEAD;
bool hasData = false;
for (size_t i = 0; i < payloadLen; i++) {
@@ -277,9 +317,9 @@ bool LD2453Parser::tryParseFrame() {
Serial.print(F("[FRAME 0x"));
if (frameType < 0x10) Serial.print('0');
Serial.print(frameType, HEX);
Serial.print(F(" payload="));
Serial.print(payloadLen);
Serial.println(F("B] — parsing into live state (user policy: never drop frame)"));
Serial.print(F(" len=")); Serial.print(frameLen);
Serial.print(F("B count=")); Serial.print(reportedCount);
Serial.println(F(" — parsing variable-length frame (never drop whole)"));
Serial.print(F("[DATA] "));
for (size_t i = 0; i < payloadLen; i++) {
if (data[i] < 0x10) Serial.print('0');
@@ -295,36 +335,46 @@ bool LD2453Parser::tryParseFrame() {
_lastFrame.targetCount = 0u;
for (uint8_t t = 0; t < MAX_TARGETS; t++) {
uint8_t *targetData = data + (t * TARGET_RECORD_SIZE);
const bool slotPresentInFrame = (t < reportedCount);
if (hasData && _rawDebug) {
int32_t x = 0;
int32_t y = 0;
int16_t speed = 0;
uint16_t resolution = 0u;
if (slotPresentInFrame) {
uint8_t *targetData = slotAreaStart + ((size_t)t * TARGET_RECORD_SIZE);
if (hasData && _rawDebug) {
Serial.print(F("[SLOT "));
Serial.print(t + 1);
Serial.print(F("] "));
for (size_t i = 0; i < TARGET_RECORD_SIZE; i++) {
if (targetData[i] < 0x10) Serial.print('0');
Serial.print(targetData[i], HEX);
Serial.print(' ');
}
}
x = decodeCoordSM16(targetData[0], targetData[1]);
y = decodeCoordSM16(targetData[2], targetData[3]);
speed = (int16_t)(targetData[4] | (targetData[5] << 8));
resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
} else if (hasData && _rawDebug) {
Serial.print(F("[SLOT "));
Serial.print(t + 1);
Serial.print(F("] "));
for (size_t i = 0; i < TARGET_RECORD_SIZE; i++) {
if (targetData[i] < 0x10) Serial.print('0');
Serial.print(targetData[i], HEX);
Serial.print(' ');
}
Serial.print(F("] --- NOT IN FRAME (count="));
Serial.print(reportedCount);
Serial.print(F(")"));
}
int32_t x = decodeCoordSM16(targetData[0], targetData[1]);
int32_t y = decodeCoordSM16(targetData[2], targetData[3]);
int16_t speed = (int16_t)(targetData[4] | (targetData[5] << 8));
uint16_t resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
(void)resolution;
/* Always keep raw decoded bytes in the RadarTarget struct — user
* said "keep everything, don't drop data". */
_lastFrame.targets[t].x_mm = x;
_lastFrame.targets[t].y_mm = y;
_lastFrame.targets[t].x_mm = (int16_t)x;
_lastFrame.targets[t].y_mm = (int16_t)y;
_lastFrame.targets[t].speed = speed;
const bool slot_nonzero = (x != 0) || (y != 0);
const bool plausible = slotPhysicallyPlausible((int32_t)x, (int32_t)y);
const bool slot_nonzero = slotPresentInFrame && ((x != 0) || (y != 0));
const bool plausible = slotPresentInFrame && slotPhysicallyPlausible((int32_t)x, (int32_t)y);
if (plausible) {
/* Normal plausible target — fully expose on all outputs. */
_lastFrame.targets[t].valid = true;
const float fx = (float)x;
const float fy = (float)y;
@@ -343,16 +393,6 @@ bool LD2453Parser::tryParseFrame() {
Serial.println(F("mm"));
}
} else if (slot_nonzero) {
// ==============================================================
// USER-REQUESTED RULE (B): slot has raw bytes (not empty), but
// decoded coords are physically IMPOSSIBLE (Y negative / >6m /
// |X| >6m etc).
// ACTION 1 – WARN on Serial Monitor.
// ACTION 2 – mark valid=false (so WebSocket/OLED/UI ignore it).
// ACTION 3 – bump badSlotWarnings counter.
// Raw bytes are still kept in targets[t].x_mm / y_mm if someone
// wants to inspect them locally later.
// ==============================================================
_lastFrame.targets[t].valid = false;
_lastFrame.targets[t].distance_mm = 0.0f;
_lastFrame.targets[t].angle_deg = 0.0f;
@@ -376,7 +416,6 @@ bool LD2453Parser::tryParseFrame() {
Serial.println(F(" — see WARN line above"));
}
} else {
/* Empty slot — zero everything, no warning needed. */
_lastFrame.targets[t].valid = false;
_lastFrame.targets[t].distance_mm = 0.0f;
_lastFrame.targets[t].angle_deg = 0.0f;
@@ -386,6 +425,11 @@ bool LD2453Parser::tryParseFrame() {
}
}
// Promote all bytes of this fully-validated variable-length frame out
// of the ring buffer so the next iteration looks for the next header.
memmove(_ringBuffer, _ringBuffer + headerPos + frameLen, _bufferLen - headerPos - frameLen);
_bufferLen -= (headerPos + frameLen);
_stats.lastFrameLength = frameLen;
if (_rawDebug) {
@@ -394,13 +438,10 @@ bool LD2453Parser::tryParseFrame() {
Serial.print(frameType, HEX);
Serial.print(F("] ACCEPTED (promoted) seq="));
Serial.print(_lastFrame.sequence_number);
Serial.print(F(" reported_count=")); Serial.print(reportedCount);
Serial.print(F(" valid_targets=")); Serial.print(_lastFrame.targetCount);
Serial.print(F(" bad_slot_warnings_total=")); Serial.println(_stats.badSlotWarnings);
}
const size_t consumedBytes = headerPos + frameLen;
memmove(_ringBuffer, _ringBuffer + consumedBytes, _bufferLen - consumedBytes);
_bufferLen -= consumedBytes;
return true; /* tell caller: fresh frame ready for WebSocket / OLED / ZoneMgr */
}
esp32s3-ld2453/OLEDDisplay.cpp
+4 -2
View File
@@ -137,8 +137,10 @@ void OLEDDisplay::updateRadarInfo(const RadarFrame &frame, const LD2453Stats &st
_display.print("Targets: ");
_display.print(frame.targetCount);
// Hiển thị các target nếu có
for (uint8_t i = 0; i < frame.targetCount && i < 3; i++) {
// Hiển thị các target nếu có — iterate all 3 slots because
// frame.targetCount != slot index (e.g. slot#3 can be valid while
// slot#1 is empty and frame.targetCount == 1).
for (uint8_t i = 0; i < 3; i++) {
if (frame.targets[i].valid) {
_display.print(" T");
_display.print(i + 1);
esp32s3-ld2453/ZoneManager.cpp
+1 -1
View File
@@ -21,7 +21,7 @@ bool ZoneManager::pointInZone(int x, int y, const Zone &z) {
void ZoneManager::update(const RadarFrame &frame) {
bool temp[MAX_ZONES] = { false, false, false };
for (uint8_t t = 0; t < frame.targetCount && t < 3; t++) {
for (uint8_t t = 0; t < 3; t++) {
const RadarTarget &target = frame.targets[t];
if (!target.valid) continue;