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() {
-    // -----------------------------------------------------------
-    // STUB: real frame sync/parse goes here once the official
-    // 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;
+    // Cần ít nhất đủ bytes cho 1 frame hoàn chỉnh
+    if (_bufferLen < FRAME_LENGTH) {
+        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;
 }