fix detection 1 id only

LD2453Parser.cpp

@@ -153,6 +153,21 @@ bool LD2453Parser::tryParseFrame() {
     // Frame hợp lệ! Parse data
     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++) {
+        if (data[i] != 0) { hasData = true; break; }
+    }
+    if (hasData && _rawDebug) {
+        Serial.print(F("[DATA SECTION] "));
+        for (size_t i = 0; i < DATA_SIZE; 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;
@@ -163,18 +178,37 @@ bool LD2453Parser::tryParseFrame() {
     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):
+        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.println();
+        }
+        
+        // Byte layout (v1 - giả định LD2450 style):
         // [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
+        // [6]:   Valid flag / target ID
+        // [7]:   Reserved
         
+        uint8_t validByte = targetData[6];
         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);
+        // Target valid: có valid byte != 0 HOẶC có tọa độ/speed != 0
+        bool valid = (validByte != 0) || (x != 0 || y != 0 || speed != 0);
+        
+        // Nếu valid byte khác 0 nhưng tọa độ 0 thì vẫn valid (target đứng yên)
+        if (validByte != 0 && (x == 0 && y == 0 && speed == 0)) {
+            valid = false; // Không valid nếu có flag nhưng tọa độ 0
+        }
         
         _lastFrame.targets[t].valid = valid;
         _lastFrame.targets[t].x_mm = x;
@@ -182,10 +216,25 @@ bool LD2453Parser::tryParseFrame() {
         _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;
+            if (x != 0 || y != 0) {
+                _lastFrame.targets[t].distance_mm = sqrt((float)x*x + (float)y*y);
+                _lastFrame.targets[t].angle_deg = atan2((float)x, (float)y) * 180.0 / PI;
+            } else {
+                _lastFrame.targets[t].distance_mm = 0;
+                _lastFrame.targets[t].angle_deg = 0;
+            }
             _lastFrame.targetCount++;
+            
+            if (_rawDebug) {
+                Serial.print(F("  -> Valid! X="));
+                Serial.print(x);
+                Serial.print(F(" Y="));
+                Serial.print(y);
+                Serial.print(F(" Speed="));
+                Serial.print(speed);
+                Serial.print(F(" ValidByte=0x"));
+                Serial.println(validByte, HEX);
+            }
         } else {
             _lastFrame.targets[t].distance_mm = 0;
             _lastFrame.targets[t].angle_deg = 0;