diff --git a/LD2453Parser.cpp b/LD2453Parser.cpp index 177f87b..250b518 100644 --- a/LD2453Parser.cpp +++ b/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; diff --git a/WebServerManager.cpp b/WebServerManager.cpp index ef053ea..c744fb9 100644 --- a/WebServerManager.cpp +++ b/WebServerManager.cpp @@ -101,9 +101,12 @@ void WebServerManager::broadcastFrame(const RadarFrame &frame) { doc["node_id"] = frame.node_id; doc["sequence"] = frame.sequence_number; doc["timestamp"] = frame.timestamp_ms; + doc["target_count"] = frame.targetCount; JsonArray targets = doc["targets"].to(); - for (uint8_t i = 0; i < frame.targetCount && i < 3; i++) { + // Loop qua TẤT CẢ 3 slot, không chỉ dựa vào targetCount + // Đảm bảo target ở slot 2 vẫn được gửi ngay cả khi slot 1 trống + for (uint8_t i = 0; i < 3; i++) { const RadarTarget &t = frame.targets[i]; JsonObject jt = targets.add(); jt["id"] = i + 1; // radar slot index, NOT a persistent person ID @@ -111,6 +114,10 @@ void WebServerManager::broadcastFrame(const RadarFrame &frame) { jt["x"] = t.x_mm; jt["y"] = t.y_mm; jt["speed"] = t.speed; + if (t.valid) { + jt["distance"] = (int)t.distance_mm; + jt["angle"] = (int)t.angle_deg; + } } String out;