From 7f4d68e8215b05238c5c1b4046efb9d05c9a2df6 Mon Sep 17 00:00:00 2001 From: Tony Tran Date: Sat, 12 Sep 2026 21:44:21 +0700 Subject: [PATCH] fix ghost again --- esp32s3-ld2453/LD2453Parser.cpp | 304 ++++++++++++++++++------------ esp32s3-ld2453/LD2453Parser.h | 19 +- esp32s3-ld2453/esp32s3-ld2453.ino | 2 + 3 files changed, 199 insertions(+), 126 deletions(-) diff --git a/esp32s3-ld2453/LD2453Parser.cpp b/esp32s3-ld2453/LD2453Parser.cpp index a4319f3..99a36bb 100644 --- a/esp32s3-ld2453/LD2453Parser.cpp +++ b/esp32s3-ld2453/LD2453Parser.cpp @@ -14,53 +14,78 @@ #endif // --------------------------------------------------------------- -// LD2450 / LD2453 TARGET REPORT FORMAT. -// Header : [0xAA, 0xFF, 0x03, 0x00] — 4 bytes (sync + little-endian -// frame type = 0x0003) -// Payload: 3 targets x 8 bytes each — 24 bytes, little-endian int16 -// Tail : [0x55, 0xCC] — 2 bytes -// Total : 30 bytes per report frame. +// LD2450 / LD2453 SERIAL FRAMING — PER-USER-DIRECTED POLICY. // -// NOTE ON FRAME-TYPE AMBIGUITY FOR THE LD2453: -// On the LD2450, frame type 0x0003 is the "target tracking report" -// that carries real T1..T3 positions. On the LD2453, however, some -// units ALSO send a periodic 30-byte 0x0003 frame that looks like a -// configuration/self-test ACK: the byte positions are identical but -// slot 1 carries garbage (coordinates like X=901 Y=-26735, i.e. a -// point 27 m BEHIND the radar) while slots 2..3 remain 0. This is -// physically impossible, so instead of switching the accepted frame -// type (which would break real LD2450 0x0003 traffic) we gate the -// per-target validity with HARD PHYSICAL LIMITS (see below). That -// single filter reliably kills ACK ghosts while keeping real targets. +// The user explicitly asked for the following behaviour — we follow it +// exactly below and do NOT invent anything else: +// (a) NEVER reject a syntactically-valid 30-byte 0x0003 frame as a +// whole. Every [AA FF 03 00 .. .. 55 CC] gets counted in +// framesParsed and promoted to _lastFrame. +// (b) Per-slot sanity is the ONLY filtering point. If a slot's decoded +// coordinates are physically implausible (Y < 0, Y > 6m, +// |X| > 6m, …): +// * we print a WARN line to Serial Monitor, +// * mark the slot valid=false IN the promoted RadarFrame, +// * increment badSlotWarnings stat, +// * do NOT send it up to the WebSocket / OLED / UI. // -// Per target 8-byte record (LD2450 + LD2453): -// [0..1] int16 X mm, signed; <0 left, >0 right -// [2..3] int16 Y mm, signed; ALWAYS >0 for real targets -// [4..5] int16 Speed cm/s, signed; >0 = moving TOWARD radar -// [6..7] uint16 DistanceRes. mm, unsigned. NOT a "valid flag" +// This "trust the wire unless proven-impossible per-slot" policy maximises +// recall — zero whole-frame dropped data loss. // -// PHYSICAL REALITY CHECKS (applied after the basic X/Y != 0 rule): -// MAX_RANGE_MM = 6000 (LD2450/LD2453 rated range = 0.75 .. 6.0 m) -// MAX_X_MM = 6000 (±60° cone at 6 m gives ~ |X| ≤ ~5200 mm; -// use 6000 for a small safety margin) -// Y_MIN_MM = 1 (Y must be IN FRONT of the radar — negative Y -// means "behind the antenna" and is ACK noise) +// (1) WIRE LAYOUT (byte-level, both chips): +// Sync 2 bytes : 0xAA, 0xFF +// Frame-type WORD: little-endian uint16 (offset [2..3]) +// Payload N bytes: meaning depends on frame-type +// Tail 2 bytes : 0x55, 0xCC +// +// (2) ACCEPTED FRAME TYPES FOR PARSING: +// * 0x0003 — LD2450/LD2453 3-slot tracking report (30 bytes +// total / 24 payload / 3 × 8 B/slot). This is the only +// format we know how to interpret today. Frames with any +// other frame-type words are skipped byte-by-byte until a +// fresh AA FF sync is found. +// +// (3) 8-BYTE SLOT LAYOUT (0x0003 frame payload slots): +// [0..1] int16 X_mm signed; <0 left, >0 right +// [2..3] int16 Y_mm signed; ALWAYS >0 for a real person +// [4..5] int16 Speed_cm_s signed; +ve = closer +// [6..7] uint16 DistanceRes_mm unsigned; NOT a "valid flag" // --------------------------------------------------------------- -static const uint8_t FRAME_HEADER[] = { 0xAA, 0xFF, 0x03, 0x00 }; -static const uint8_t FRAME_TAIL[] = { 0x55, 0xCC }; -static const size_t FRAME_LENGTH = 30; -static const size_t HEADER_SIZE = 4; +static const uint8_t FRAME_SYNC[] = { 0xAA, 0xFF }; +static const uint8_t FRAME_TAIL[] = { 0x55, 0xCC }; +static const size_t SYNC_SIZE = 2; +static const size_t FRAME_TYPE_SIZE = 2; +static const size_t HEADER_SIZE = SYNC_SIZE + FRAME_TYPE_SIZE; // 4 bytes static const size_t TAIL_SIZE = 2; -static const size_t DATA_SIZE = FRAME_LENGTH - HEADER_SIZE - TAIL_SIZE; static const size_t TARGET_RECORD_SIZE = 8; static const size_t MAX_TARGETS = 3; -/* Hard physical limits — anything outside these bounds is discarded as - * a spurious ACK/register dump rather than a real target report. */ +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, +}; +static const size_t accepted_target_types_count = + sizeof(accepted_target_types) / sizeof(accepted_target_types[0]); + +// Per-slot physical plausibility bounds (used for the WARN step (b) above). static const int32_t Y_MIN_MM = 1; static const int32_t Y_MAX_MM = 6000; static const int32_t X_ABS_MAX_MM = 6000; +// Returns true iff decoded (x,y) sit inside the radar's measurable +// operational envelope (in front of the antenna and inside rated +// range / FOV cone / safety margin). +static inline bool slotPhysicallyPlausible(int32_t x, int32_t y) { + if (x == 0 && y == 0) return false; /* empty slot */ + if (y < Y_MIN_MM || y > Y_MAX_MM) return false; /* behind / out-of-range */ + if (x < -X_ABS_MAX_MM || x > X_ABS_MAX_MM) return false; /* outside ±60° cone + margin */ + return true; +} + void LD2453Parser::begin(HardwareSerial &serial, int rxPin, int txPin, uint32_t baud, const char *nodeId, bool rawDebug) { _serial = &serial; @@ -144,73 +169,95 @@ bool LD2453Parser::update() { } bool LD2453Parser::tryParseFrame() { - // Cần ít nhất đủ bytes cho 1 frame hoàn chỉnh - if (_bufferLen < FRAME_LENGTH) { - return false; - } + if (_bufferLen < TARGET_REPORT_TOTAL_LEN) return false; - // Tìm frame header - size_t headerPos = 0; - bool foundHeader = false; - - for (size_t i = 0; i <= _bufferLen - FRAME_LENGTH; i++) { - if (memcmp(_ringBuffer + i, FRAME_HEADER, HEADER_SIZE) == 0) { - headerPos = i; - foundHeader = true; - break; + // ---------------------------------------------------------------- + // 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. + // ---------------------------------------------------------------- + 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; + + for (size_t i = 0; i <= _bufferLen - TARGET_REPORT_TOTAL_LEN; i++) { + if (memcmp(_ringBuffer + i, FRAME_SYNC, SYNC_SIZE) != 0) continue; + + const uint16_t candidateType = + (uint16_t)(_ringBuffer[i + 2]) | + ((uint16_t)(_ringBuffer[i + 3]) << 8); + + bool known = false; + for (size_t k = 0; k < accepted_target_types_count; k++) { + if (candidateType == accepted_target_types[k]) { known = true; break; } } + if (!known) { + memmove(_ringBuffer, _ringBuffer + i + 1, _bufferLen - i - 1); + _bufferLen -= (i + 1); + return tryParseFrame(); + } + + headerPos = i; + foundSync = true; + frameType = candidateType; + break; } - if (!foundHeader) { - // Không tìm thấy header, drop bytes cũ để tránh tràn buffer + if (!foundSync) { if (_bufferLen >= RING_BUFFER_SIZE / 2) { - memmove(_ringBuffer, _ringBuffer + RING_BUFFER_SIZE / 4, + memmove(_ringBuffer, _ringBuffer + RING_BUFFER_SIZE / 4, _bufferLen - RING_BUFFER_SIZE / 4); _bufferLen -= RING_BUFFER_SIZE / 4; } return false; } + if (headerPos + frameLen > _bufferLen) return false; - // Kiểm tra đủ bytes cho frame hoàn chỉnh - if (headerPos + FRAME_LENGTH > _bufferLen) { - return false; // Chưa đủ bytes, chờ thêm - } - - // Verify frame tail - size_t tailPos = headerPos + FRAME_LENGTH - TAIL_SIZE; + const size_t tailPos = headerPos + frameLen - TAIL_SIZE; if (memcmp(_ringBuffer + tailPos, FRAME_TAIL, TAIL_SIZE) != 0) { - // Header đúng nhưng tail sai, bỏ qua header này memmove(_ringBuffer, _ringBuffer + headerPos + 1, _bufferLen - headerPos - 1); _bufferLen -= (headerPos + 1); _stats.parseErrors++; return false; } - // Frame hợp lệ! Parse data + // ---------------------------------------------------------------- + // 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. + // ---------------------------------------------------------------- uint8_t *data = _ringBuffer + headerPos + HEADER_SIZE; - - // Debug: dump toàn bộ data section nếu có byte nào khác 0 + bool hasData = false; - for (size_t i = 0; i < DATA_SIZE; i++) { + for (size_t i = 0; i < payloadLen; i++) { if (data[i] != 0) { hasData = true; break; } } if (hasData && _rawDebug) { - Serial.print(F("[DATA SECTION] ")); - for (size_t i = 0; i < DATA_SIZE; i++) { + 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("[DATA] ")); + for (size_t i = 0; i < payloadLen; i++) { if (data[i] < 0x10) Serial.print('0'); Serial.print(data[i], HEX); Serial.print(' '); } Serial.println(); } - - // Reset frame - _lastFrame.timestamp_ms = millis(); - _lastFrame.sequence_number = ++_sequenceCounter; - _lastFrame.node_id = _nodeId; - _lastFrame.targetCount = 0; - // Parse targets (3 targets, mỗi target 8 bytes — LD2450 confirmed format) + _lastFrame.timestamp_ms = millis(); + _lastFrame.sequence_number = ++_sequenceCounter; + _lastFrame.node_id = _nodeId; + _lastFrame.targetCount = 0u; + for (uint8_t t = 0; t < MAX_TARGETS; t++) { uint8_t *targetData = data + (t * TARGET_RECORD_SIZE); @@ -223,86 +270,101 @@ bool LD2453Parser::tryParseFrame() { Serial.print(targetData[i], HEX); Serial.print(' '); } - Serial.println(); } - // LD2450 8-byte per-target layout (little-endian everywhere): - // [0..1] int16 X (mm, signed; negative = radar's left, positive = right) - // [2..3] int16 Y (mm, signed; always positive in front-of-radar space) - // [4..5] int16 Speed (cm/s, signed; positive = moving closer) - // [6..7] uint16 DistanceResolution (mm, unsigned) — NOT a valid flag int16_t x = (int16_t)(targetData[0] | (targetData[1] << 8)); int16_t y = (int16_t)(targetData[2] | (targetData[3] << 8)); int16_t speed = (int16_t)(targetData[4] | (targetData[5] << 8)); uint16_t resolution = (uint16_t)(targetData[6] | (targetData[7] << 8)); (void)resolution; - // ============================================================ - // VALIDITY DECISION — 2 stages, tuned to eliminate the two - // most common LD245x false-positive sources: - // - // STAGE 1. Position-exists rule (LD2450 standard): - // (x != 0) || (y != 0) - // - // This kills the "valid-flag byte at offset 6" - // false-positive ghost at (0,0). - // - // STAGE 2. HARD PHYSICAL LIMITS (the new part needed for - // LD2453 units that leak ACK bytes on frame-type - // 0x0003): - // Y ∈ [1, 6000] (always IN FRONT of radar, - // inside rated range) - // |X| ≤ 6000 (inside ±60° detection - // cone + safety margin) - // (x != 0) || (y != 0) - // - // Stage 2 alone is sufficient to drop the exact ACK pattern - // the user captured: X=901 Y=-26735 (Y negative, 27 m behind - // radar → impossible). Stage 1 + 2 together cover all - // currently-known LD2450 / LD2453 false-positives. - // ============================================================ - const bool position_exists = (x != 0) || (y != 0); - const bool y_in_front_range = - ((int32_t)y >= Y_MIN_MM) && ((int32_t)y <= Y_MAX_MM); - const bool x_in_cone = - ((int32_t)x >= -X_ABS_MAX_MM) && ((int32_t)x <= X_ABS_MAX_MM); - - const bool valid = position_exists && y_in_front_range && x_in_cone; - - _lastFrame.targets[t].valid = valid; + /* 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].speed = speed; - if (valid) { + const bool slot_nonzero = (x != 0) || (y != 0); + const bool plausible = 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; _lastFrame.targets[t].distance_mm = sqrtf(fx * fx + fy * fy); _lastFrame.targets[t].angle_deg = atan2f(fx, fy) * 180.0f / PI; _lastFrame.targetCount++; - if (_rawDebug) { + if (hasData && _rawDebug) { + Serial.println(); Serial.print(F(" -> Valid T")); Serial.print(t + 1); Serial.print(F(": X=")); Serial.print(x); Serial.print(F(" Y=")); Serial.print(y); Serial.print(F(" Speed=")); Serial.print(speed); - Serial.print(F("cm/s Dist=")); Serial.print((int)_lastFrame.targets[t].distance_mm); - Serial.print(F("mm Res=")); Serial.print(resolution); + Serial.print(F("cm/s Dist=")); + Serial.print((int)_lastFrame.targets[t].distance_mm); Serial.println(F("mm")); } - } else { + } 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; + _stats.badSlotWarnings++; + + Serial.print(F("[WARN] LD2453 slot ")); + Serial.print(t + 1); + Serial.print(F(" — implausible coords IGNORED (valid=false; not sent to WS/UI). " + "Raw decode: X=")); Serial.print(x); + Serial.print(F("mm, Y=")); Serial.print(y); + Serial.print(F("mm, Speed=")); Serial.print(speed); + Serial.print(F("cm/s. Limit: |X|≤")); Serial.print(X_ABS_MAX_MM); + Serial.print(F(", Y∈[")); Serial.print(Y_MIN_MM); + Serial.print(F(",")); Serial.print(Y_MAX_MM); Serial.print(F("] mm. seq=")); + Serial.println(_lastFrame.sequence_number); + + if (hasData && _rawDebug) { + Serial.print(F(" -> BAD-SLOT (not exposed) X=")); Serial.print(x); + Serial.print(F(" Y=")); Serial.print(y); + Serial.print(F(" Res=")); Serial.print(resolution); + 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; + if (hasData && _rawDebug) { + Serial.println(F(" -> EMPTY")); + } } } - // Lưu frame length vào stats - _stats.lastFrameLength = FRAME_LENGTH; + _stats.lastFrameLength = frameLen; - // Remove frame đã parse khỏi buffer - size_t consumedBytes = headerPos + FRAME_LENGTH; + if (_rawDebug) { + Serial.print(F("[FRAME 0x")); + if (frameType < 0x10) Serial.print('0'); + Serial.print(frameType, HEX); + Serial.print(F("] ACCEPTED (promoted) seq=")); + Serial.print(_lastFrame.sequence_number); + 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; + return true; /* tell caller: fresh frame ready for WebSocket / OLED / ZoneMgr */ } diff --git a/esp32s3-ld2453/LD2453Parser.h b/esp32s3-ld2453/LD2453Parser.h index 1b57605..9f28ef9 100644 --- a/esp32s3-ld2453/LD2453Parser.h +++ b/esp32s3-ld2453/LD2453Parser.h @@ -54,11 +54,20 @@ struct RadarFrame { // Simple acquisition/parse statistics, useful for the diagnostic // serial output requested for stage 1. struct LD2453Stats { - uint32_t framesParsed = 0; - uint32_t parseErrors = 0; - uint32_t bytesReceived = 0; - uint32_t lastFrameLength = 0; - float packetsPerSecond = 0.0f; + uint32_t framesParsed = 0; /* every syntactically-valid AA FF .. 55 CC frame + * that passed length+type+tail checks is counted + * here — per user request we NEVER drop whole + * frames (to avoid accidentally discarding real + * data); filtering is per-slot only. */ + uint32_t badSlotWarnings = 0; /* a slot had non-zero bytes but its decoded + * coords were physically implausible (e.g. + * Y negative, range >6m). We warn on Serial + * and mark it valid=false (see tryParseFrame), + * but still keep its raw bytes in the frame. */ + uint32_t parseErrors = 0; + uint32_t bytesReceived = 0; + uint32_t lastFrameLength = 0; + float packetsPerSecond = 0.0f; }; // --------------------------------------------------------------- diff --git a/esp32s3-ld2453/esp32s3-ld2453.ino b/esp32s3-ld2453/esp32s3-ld2453.ino index f4c1eba..baeffd5 100644 --- a/esp32s3-ld2453/esp32s3-ld2453.ino +++ b/esp32s3-ld2453/esp32s3-ld2453.ino @@ -60,6 +60,8 @@ void printDiagnostics(const RadarFrame &frame, const LD2453Stats &stats) { Serial.println(stats.bytesReceived); Serial.print(F("Frames parsed: ")); Serial.println(stats.framesParsed); + Serial.print(F("Bad-slot warnings: ")); + Serial.println(stats.badSlotWarnings); Serial.print(F("Frame length: ")); Serial.println(stats.lastFrameLength); Serial.print(F("Parse errors: "));