update 3 targets

esp32s3-ld2453/LD2453Parser.cpp

@@ -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
+    // STEP 1 – Locate [0xAA 0xFF] sync, decode the uint16 LE frame-type,
-    // that follows, keep only types we know how to interpret today.
+    // then read the Group + Count bytes to know how many target records
-    // Per user request, KNOWN frames (type 0x0003) will ALL be promoted
+    // actually follow (the radar sends VARIABLE-length frames, NOT an
-    // to the live RadarState — filtering is per-slot WARN below, never
+    // always-3-slot fixed 24-byte payload).
-    // 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   frameLen      = 0u;
-    size_t   payloadLen = TARGET_REPORT_PAYLOAD_LEN;
+    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
+    //   * Slots 0 .. reportedCount-1 come from the real payload bytes
-    // and bump sequence).  We only filter at the per-slot level via
+    //     starting at offset PAYLOAD_HEADER_OVERHEAD (after Group+Count).
-    // the WARN rule below.
+    //   * 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 *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(F("  len=")); Serial.print(frameLen);
-        Serial.print(payloadLen);
+        Serial.print(F("B  count=")); Serial.print(reportedCount);
-        Serial.println(F("B] — parsing into live state (user policy: never drop frame)"));
+        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,8 +335,15 @@ 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);
 
+        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);
@@ -307,24 +354,27 @@ bool LD2453Parser::tryParseFrame() {
                     Serial.print(' ');
                 }
             }
-
+            x          = decodeCoordSM16(targetData[0], targetData[1]);
-        int32_t  x          = decodeCoordSM16(targetData[0], targetData[1]);
+            y          = decodeCoordSM16(targetData[2], targetData[3]);
-        int32_t  y          = decodeCoordSM16(targetData[2], targetData[3]);
+            speed      = (int16_t)(targetData[4] | (targetData[5] << 8));
-        int16_t  speed      = (int16_t)(targetData[4] | (targetData[5] << 8));
+            resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
-        uint16_t resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
+        } else if (hasData && _rawDebug) {
+            Serial.print(F("[SLOT "));
+            Serial.print(t + 1);
+            Serial.print(F("] --- NOT IN FRAME (count="));
+            Serial.print(reportedCount);
+            Serial.print(F(")"));
+        }
         (void)resolution;
 
-        /* Always keep raw decoded bytes in the RadarTarget struct — user
+        _lastFrame.targets[t].x_mm  = (int16_t)x;
-         * said "keep everything, don't drop data". */
+        _lastFrame.targets[t].y_mm  = (int16_t)y;
-        _lastFrame.targets[t].x_mm  = x;
-        _lastFrame.targets[t].y_mm  = y;
         _lastFrame.targets[t].speed = speed;
 
-        const bool slot_nonzero = (x != 0) || (y != 0);
+        const bool slot_nonzero = slotPresentInFrame && ((x != 0) || (y != 0));
-        const bool plausible   = slotPhysicallyPlausible((int32_t)x, (int32_t)y);
+        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

@@ -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ó
+    // Hiển thị các target nếu có — iterate all 3 slots because
-    for (uint8_t i = 0; i < frame.targetCount && i < 3; i++) {
+    // 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

@@ -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;