diff --git a/esp32s3-ld2453/LD2453Parser.cpp b/esp32s3-ld2453/LD2453Parser.cpp index 5152f52..fb89ff4 100644 --- a/esp32s3-ld2453/LD2453Parser.cpp +++ b/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 - // 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 */ } diff --git a/esp32s3-ld2453/OLEDDisplay.cpp b/esp32s3-ld2453/OLEDDisplay.cpp index 2280062..ee71fee 100644 --- a/esp32s3-ld2453/OLEDDisplay.cpp +++ b/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ó - 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); diff --git a/esp32s3-ld2453/ZoneManager.cpp b/esp32s3-ld2453/ZoneManager.cpp index 69cda84..5a34065 100644 --- a/esp32s3-ld2453/ZoneManager.cpp +++ b/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;