implement more

LD2453Parser.cpp

@@ -81,6 +81,7 @@ bool LD2453Parser::update() {
         uint8_t b = (uint8_t)_serial->read();
         feedByte(b);
         _stats.bytesReceived++;
+        _lastByteTime = millis();  // Cập nhật thời điểm nhận byte
     }
     if (_rawDebug && _bufferLen > newBytesStart) {
         dumpRawHex(_ringBuffer + newBytesStart, _bufferLen - newBytesStart);
@@ -107,27 +108,97 @@ bool LD2453Parser::update() {
 }
 
 bool LD2453Parser::tryParseFrame() {
-    // -----------------------------------------------------------
+    // Cần ít nhất đủ bytes cho 1 frame hoàn chỉnh
-    // STUB: real frame sync/parse goes here once the official
+    if (_bufferLen < FRAME_LENGTH) {
-    // HLK-LD2453 protocol is confirmed. Suggested shape, mirroring
-    // the LD2410/LD2450 family's approach (adapt once verified):
-    //
-    //   1. Scan _ringBuffer for FRAME_HEADER.
-    //   2. If found but fewer than (header + declared length + tail)
-    //      bytes are available yet, return false and wait for more
-    //      bytes next call (do NOT block).
-    //   3. Verify FRAME_TAIL at the expected offset.
-    //   4. If rawDebug, dumpRawHex() the whole candidate frame.
-    //   5. Decode each target record into RadarTarget (x_mm, y_mm,
-    //      speed, and distance/angle only if the protocol actually
-    //      carries them).
-    //   6. Fill _lastFrame (timestamp_ms = millis(), sequence_number
-    //      = ++_sequenceCounter, node_id = _nodeId, targetCount).
-    //   7. memmove() the consumed bytes out of _ringBuffer.
-    //   8. Return true.
-    //
-    // Until then, this always returns false so callers never receive
-    // fabricated coordinates.
-    // -----------------------------------------------------------
         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;
+        }
+    }
+
+    if (!foundHeader) {
+        // 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, 
+                    _bufferLen - RING_BUFFER_SIZE / 4);
+            _bufferLen -= RING_BUFFER_SIZE / 4;
+        }
+        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;
+    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
+    uint8_t *data = _ringBuffer + headerPos + HEADER_SIZE;
+    
+    // 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)
+    for (uint8_t t = 0; t < MAX_TARGETS; t++) {
+        uint8_t *targetData = data + (t * TARGET_RECORD_SIZE);
+        
+        // Byte layout (giả định, cần verify với manual):
+        // [0-1]: X (little endian, signed 16-bit, mm)
+        // [2-3]: Y (little endian, signed 16-bit, mm)
+        // [4-5]: Speed (little endian, signed 16-bit, cm/s)
+        // [6-7]: Reserved hoặc distance
+        
+        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));
+        
+        // Target valid nếu không phải tất cả đều 0
+        bool valid = (x != 0 || y != 0 || speed != 0);
+        
+        _lastFrame.targets[t].valid = valid;
+        _lastFrame.targets[t].x_mm = x;
+        _lastFrame.targets[t].y_mm = y;
+        _lastFrame.targets[t].speed = speed;
+        
+        if (valid) {
+            // Tính distance và angle từ x, y
+            _lastFrame.targets[t].distance_mm = sqrt(x*x + y*y);
+            _lastFrame.targets[t].angle_deg = atan2(x, y) * 180.0 / PI;
+            _lastFrame.targetCount++;
+        } else {
+            _lastFrame.targets[t].distance_mm = 0;
+            _lastFrame.targets[t].angle_deg = 0;
+        }
+    }
+
+    // Lưu frame length vào stats
+    _stats.lastFrameLength = FRAME_LENGTH;
+
+    // Remove frame đã parse khỏi buffer
+    size_t consumedBytes = headerPos + FRAME_LENGTH;
+    memmove(_ringBuffer, _ringBuffer + consumedBytes, _bufferLen - consumedBytes);
+    _bufferLen -= consumedBytes;
+
+    return true;
 }

LD2453Parser.h

@@ -94,6 +94,7 @@ private:
     RadarFrame _lastFrame{};
     LD2453Stats _stats{};
     uint32_t _sequenceCounter = 0;
+    uint32_t _lastByteTime = 0;  // Thời điểm nhận byte cuối cùng
 
     uint32_t _ppsWindowStart = 0;
     uint32_t _ppsWindowCount = 0;

config.h

@@ -56,4 +56,4 @@
 //     stage — get the parser confirmed against real hardware before
 //     bringing up networking.
 // 0 = full stage-2 firmware: Wi-Fi + REST API + WebSocket + zones.
-#define DIAGNOSTIC_MODE 0
+#define DIAGNOSTIC_MODE 1

esp32-s3-ld2453.ino

@@ -46,10 +46,21 @@ WebServerManager webServer(zoneManager);
 
 void printDiagnostics(const RadarFrame &frame, const LD2453Stats &stats) {
     Serial.println();
-    Serial.println(F("LD2453 connected"));
+    Serial.println(F("========================================"));
-    Serial.print(F("UART: "));
+    Serial.print(F("LD2453 Status: "));
+    if (radar.isConnected()) {
+        Serial.println(F("CONNECTED ✓"));
+    } else {
+        Serial.println(F("DISCONNECTED ✗"));
+    }
+    Serial.println(F("========================================"));
+    Serial.print(F("UART Baud: "));
     Serial.println(RADAR_BAUD_RATE);
-    Serial.print(F("Frame length (last): "));
+    Serial.print(F("Bytes received: "));
+    Serial.println(stats.bytesReceived);
+    Serial.print(F("Frames parsed: "));
+    Serial.println(stats.framesParsed);
+    Serial.print(F("Frame length: "));
     Serial.println(stats.lastFrameLength);
     Serial.print(F("Parse errors: "));
     Serial.println(stats.parseErrors);
@@ -66,18 +77,26 @@ void printDiagnostics(const RadarFrame &frame, const LD2453Stats &stats) {
 
         const RadarTarget &t = frame.targets[i];
         if (!t.valid) {
-            Serial.println(F("INVALID"));
+            Serial.println(F("  INVALID"));
             continue;
         }
-        Serial.print(F("X: "));
+        Serial.print(F("  X: "));
         Serial.print(t.x_mm);
         Serial.println(F(" mm"));
-        Serial.print(F("Y: "));
+        Serial.print(F("  Y: "));
         Serial.print(t.y_mm);
         Serial.println(F(" mm"));
-        Serial.print(F("Speed: "));
+        Serial.print(F("  Speed: "));
-        Serial.println(t.speed);
+        Serial.print(t.speed);
+        Serial.println(F(" cm/s"));
+        Serial.print(F("  Distance: "));
+        Serial.print(t.distance_mm, 1);
+        Serial.println(F(" mm"));
+        Serial.print(F("  Angle: "));
+        Serial.print(t.angle_deg, 1);
+        Serial.println(F(" deg"));
     }
+    Serial.println(F("========================================"));
 }
 
 void setup() {
@@ -166,6 +185,12 @@ void loop() {
     if (now - lastStatsReport >= 5000) { // every 5 seconds
         const LD2453Stats &stats = radar.getStats();
         Serial.println(F("\n--- Periodic Status ---"));
+        Serial.print(F("LD2453 Connection: "));
+        if (radar.isConnected()) {
+            Serial.println(F("OK ✓"));
+        } else {
+            Serial.println(F("LOST ✗"));
+        }
         Serial.print(F("Bytes received: "));
         Serial.println(stats.bytesReceived);
         Serial.print(F("Frames parsed: "));