fix ghost again

esp32s3-ld2453/LD2453Parser.cpp

@@ -14,53 +14,78 @@
 #endif
 
 // ---------------------------------------------------------------
-// LD2450 / LD2453 TARGET REPORT FORMAT.
+// LD2450 / LD2453 SERIAL FRAMING — PER-USER-DIRECTED POLICY.
-// 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.
 //
-// NOTE ON FRAME-TYPE AMBIGUITY FOR THE LD2453:
+// The user explicitly asked for the following behaviour — we follow it
-//   On the LD2450, frame type 0x0003 is the "target tracking report"
+// exactly below and do NOT invent anything else:
-// that carries real T1..T3 positions.  On the LD2453, however, some
+//   (a) NEVER reject a syntactically-valid 30-byte 0x0003 frame as a
-// units ALSO send a periodic 30-byte 0x0003 frame that looks like a
+//       whole.  Every [AA FF 03 00 .. .. 55 CC] gets counted in
-// configuration/self-test ACK: the byte positions are identical but
+//       framesParsed and promoted to _lastFrame.
-// slot 1 carries garbage (coordinates like X=901 Y=-26735, i.e. a
+//   (b) Per-slot sanity is the ONLY filtering point.  If a slot's decoded
-// point 27 m BEHIND the radar) while slots 2..3 remain 0.  This is
+//       coordinates are physically implausible (Y < 0, Y > 6m,
-// physically impossible, so instead of switching the accepted frame
+//       |X| > 6m, …):
-// type (which would break real LD2450 0x0003 traffic) we gate the
+//         * we print a WARN line to Serial Monitor,
-// per-target validity with HARD PHYSICAL LIMITS (see below).  That
+//         * mark the slot valid=false IN the promoted RadarFrame,
-// single filter reliably kills ACK ghosts while keeping real targets.
+//         * increment badSlotWarnings stat,
+//         * do NOT send it up to the WebSocket / OLED / UI.
 //
-// Per target 8-byte record (LD2450 + LD2453):
+// This "trust the wire unless proven-impossible per-slot" policy maximises
-//   [0..1]  int16  X              mm, signed; <0 left, >0 right
+// recall — zero whole-frame dropped data loss.
-//   [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"
 //
-// PHYSICAL REALITY CHECKS (applied after the basic X/Y != 0 rule):
+// (1) WIRE LAYOUT (byte-level, both chips):
-//   MAX_RANGE_MM  = 6000  (LD2450/LD2453 rated range = 0.75 .. 6.0 m)
+//     Sync 2 bytes  : 0xAA, 0xFF
-//   MAX_X_MM      = 6000  (±60° cone at 6 m gives ~ |X| ≤ ~5200 mm;
+//     Frame-type WORD: little-endian uint16 (offset [2..3])
-//                          use 6000 for a small safety margin)
+//     Payload N bytes: meaning depends on frame-type
-//   Y_MIN_MM      = 1     (Y must be IN FRONT of the radar — negative Y
+//     Tail 2 bytes  : 0x55, 0xCC
-//                          means "behind the antenna" and is ACK noise)
+//
+// (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_SYNC[] = { 0xAA, 0xFF };
-static const uint8_t FRAME_TAIL[]   = { 0x55, 0xCC };
+static const uint8_t FRAME_TAIL[] = { 0x55, 0xCC };
-static const size_t  FRAME_LENGTH = 30;
+static const size_t  SYNC_SIZE = 2;
-static const size_t  HEADER_SIZE = 4;
+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
+static const uint16_t FRAME_TYPE_TARGETS_LD2450 = 0x0003u;
- * a spurious ACK/register dump rather than a real target report. */
+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 < TARGET_REPORT_TOTAL_LEN) return false;
-    if (_bufferLen < FRAME_LENGTH) {
-        return false;
-    }
 
-    // Tìm frame header
+    // ----------------------------------------------------------------
-    size_t headerPos = 0;
+    // STEP 1 – Locate [0xAA 0xFF] sync, decode the uint16 LE frame-type
-    bool foundHeader = false;
+    // that follows, keep only types we know how to interpret today.
-    
+    // Per user request, KNOWN frames (type 0x0003) will ALL be promoted
-    for (size_t i = 0; i <= _bufferLen - FRAME_LENGTH; i++) {
+    // to the live RadarState — filtering is per-slot WARN below, never
-        if (memcmp(_ringBuffer + i, FRAME_HEADER, HEADER_SIZE) == 0) {
+    // whole-frame drop, so we never accidentally lose a real target.
-            headerPos = i;
+    // ----------------------------------------------------------------
-            foundHeader = true;
+    size_t   headerPos  = 0;
-            break;
+    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) {
+    if (!foundSync) {
-        // Không tìm thấy header, drop bytes cũ để tránh tràn buffer
         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
+    const size_t tailPos = headerPos + frameLen - TAIL_SIZE;
-    if (headerPos + FRAME_LENGTH > _bufferLen) {
-        return false; // Chưa đủ bytes, chờ thêm
-    }
-
-    // Verify frame tail
-    size_t tailPos = headerPos + FRAME_LENGTH - 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] "));
+        Serial.print(F("[FRAME 0x"));
-        for (size_t i = 0; i < DATA_SIZE; i++) {
+        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;
 
-        // ============================================================
+        /* Always keep raw decoded bytes in the RadarTarget struct — user
-        // VALIDITY DECISION — 2 stages, tuned to eliminate the two
+         * said "keep everything, don't drop data". */
-        // 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;
         _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("cm/s  Dist="));
-                Serial.print(F("mm  Res=")); Serial.print(resolution);
+                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 = frameLen;
-    _stats.lastFrameLength = FRAME_LENGTH;
 
-    // Remove frame đã parse khỏi buffer
+    if (_rawDebug) {
-    size_t consumedBytes = headerPos + FRAME_LENGTH;
+        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 */
 }

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 framesParsed     = 0; /* every syntactically-valid AA FF .. 55 CC frame
-    uint32_t parseErrors = 0;
+                                    * that passed length+type+tail checks is counted
-    uint32_t bytesReceived = 0;
+                                    * here — per user request we NEVER drop whole
-    uint32_t lastFrameLength = 0;
+                                    * frames (to avoid accidentally discarding real
-    float packetsPerSecond = 0.0f;
+                                    * 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;
 };
 
 // ---------------------------------------------------------------

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: "));