205 lines
7.4 KiB
C++
205 lines
7.4 KiB
C++
// =====================================================================
|
|
// LD2453Parser.cpp
|
|
//
|
|
// See the big comment at the top of LD2453Parser.h before touching
|
|
// this file. The acquisition/debug layer below is fully functional;
|
|
// tryParseFrame() is intentionally stubbed pending the official
|
|
// HLK-LD2453 protocol.
|
|
// =====================================================================
|
|
#include "LD2453Parser.h"
|
|
|
|
#if !defined(ARDUINO_ARCH_ESP32)
|
|
#warning "LD2453Parser was written/tested against the ESP32 Arduino core. Verify HardwareSerial pin-remap behavior on other cores."
|
|
#endif
|
|
|
|
// ---------------------------------------------------------------
|
|
// PLACEHOLDER PROTOCOL CONSTANTS
|
|
//
|
|
// None of the values below are confirmed. They exist only so the
|
|
// state machine in tryParseFrame() has something concrete to compare
|
|
// against once you fill them in from the real manual or from a
|
|
// DEBUG_RADAR_RAW capture. Do not rely on these for real parsing.
|
|
// ---------------------------------------------------------------
|
|
// static const uint8_t FRAME_HEADER[] = { 0x00, 0x00, 0x00, 0x00 }; // TODO: confirm
|
|
// static const uint8_t FRAME_TAIL[] = { 0x00, 0x00 }; // TODO: confirm
|
|
// static const size_t TARGET_RECORD_SIZE = 0; // bytes per target, TODO: confirm
|
|
// static const size_t MAX_TARGETS = 3;
|
|
|
|
void LD2453Parser::begin(HardwareSerial &serial, int rxPin, int txPin, uint32_t baud,
|
|
const char *nodeId, bool rawDebug) {
|
|
_serial = &serial;
|
|
_rawDebug = rawDebug;
|
|
_nodeId = nodeId;
|
|
_bufferLen = 0;
|
|
_sequenceCounter = 0;
|
|
_stats = LD2453Stats{};
|
|
_ppsWindowStart = millis();
|
|
_ppsWindowCount = 0;
|
|
|
|
// HardwareSerial::begin(baud, config, rxPin, txPin) — ESP32 Arduino
|
|
// core supports remapping RX/TX to any capable GPIO. 8N1 assumed;
|
|
// confirm against the official manual.
|
|
_serial->begin(baud, SERIAL_8N1, rxPin, txPin);
|
|
}
|
|
|
|
void LD2453Parser::dumpRawHex(const uint8_t *data, size_t len) {
|
|
Serial.print(F("[RAW] "));
|
|
for (size_t i = 0; i < len; i++) {
|
|
if (data[i] < 0x10) Serial.print('0');
|
|
Serial.print(data[i], HEX);
|
|
Serial.print(' ');
|
|
}
|
|
Serial.print(F(" (len="));
|
|
Serial.print(len);
|
|
Serial.println(')');
|
|
}
|
|
|
|
void LD2453Parser::feedByte(uint8_t b) {
|
|
if (_bufferLen >= RING_BUFFER_SIZE) {
|
|
// Buffer overrun with no recognized frame boundary — drop the
|
|
// oldest half so we can keep tolerating a stuck/garbage stream
|
|
// without ever blocking or allocating.
|
|
memmove(_ringBuffer, _ringBuffer + RING_BUFFER_SIZE / 2, RING_BUFFER_SIZE / 2);
|
|
_bufferLen = RING_BUFFER_SIZE / 2;
|
|
_stats.parseErrors++;
|
|
}
|
|
_ringBuffer[_bufferLen++] = b;
|
|
}
|
|
|
|
bool LD2453Parser::update() {
|
|
if (!_serial) return false;
|
|
|
|
bool gotFrame = false;
|
|
|
|
// Drain whatever is available right now — non-blocking, no delay().
|
|
// While rawDebug is on, dump exactly the newly-arrived bytes as
|
|
// hex so you can capture real traffic to reverse-engineer the
|
|
// frame format, independent of whether tryParseFrame() (still a
|
|
// stub) recognizes anything.
|
|
size_t newBytesStart = _bufferLen;
|
|
while (_serial->available() > 0) {
|
|
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);
|
|
}
|
|
|
|
// Try to pull as many complete frames out of the buffer as
|
|
// possible in this call (in case several arrived since the last
|
|
// loop() iteration).
|
|
while (tryParseFrame()) {
|
|
gotFrame = true;
|
|
_stats.framesParsed++;
|
|
_ppsWindowCount++;
|
|
}
|
|
|
|
// Update packets-per-second once a second, non-blocking.
|
|
uint32_t now = millis();
|
|
if (now - _ppsWindowStart >= 1000) {
|
|
_stats.packetsPerSecond = _ppsWindowCount * 1000.0f / (now - _ppsWindowStart);
|
|
_ppsWindowStart = now;
|
|
_ppsWindowCount = 0;
|
|
}
|
|
|
|
return gotFrame;
|
|
}
|
|
|
|
bool LD2453Parser::tryParseFrame() {
|
|
// 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;
|
|
}
|