fix LD2453 parser
This commit is contained in:
admin committed
1 parent
000d2e818f
commit
31c9d332ba
3 files changed
+91
-73
No files matched your search
@@ -14,7 +14,27 @@
|
||||
#endif
|
||||
|
||||
// ---------------------------------------------------------------
|
||||
// LD2453 PROTOCOL CONSTANTS (Confirmed from RAW capture)
|
||||
// LD2450 TARGET REPORT FORMAT (confirmed vs LD2450 libs on GitHub)
|
||||
// Header : [0xAA, 0xFF, 0x03, 0x00] — 4 bytes (little-endian type+len)
|
||||
// Payload: 3 targets x 8 bytes each — 24 bytes, little-endian int16 per field
|
||||
// Tail : [0x55, 0xCC] — 2 bytes
|
||||
// Total : 30 bytes per target report frame.
|
||||
//
|
||||
// Per target 8-byte record:
|
||||
// [0..1] X = (int16) LE, millimetres (signed, left/right)
|
||||
// [2..3] Y = (int16) LE, millimetres (signed, forward = +Y)
|
||||
// [4..5] Speed = (int16) LE, cm/s. Conventional sign: positive =
|
||||
// moving toward radar (decreasing Y distance).
|
||||
// [6..7] Resolution= (uint16) LE, distance resolution of the tracker
|
||||
// for this slot, in millimetres. NOT a valid flag.
|
||||
//
|
||||
// VALIDITY RULE (industry standard for LD2450 — used by every public
|
||||
// parser we cross-checked against the original ESP32-LD2450 repo):
|
||||
// Target is VALID <=> (X != 0 OR Y != 0).
|
||||
// There is NO "valid byte" at offset 6 — that byte is the low byte of
|
||||
// Resolution, which can be non-zero even for idle (all-zero) slots when
|
||||
// the DSP has no track to report. Treating it as a valid flag caused
|
||||
// the classic "radar reports 1 stationary ghost at origin" bug.
|
||||
// ---------------------------------------------------------------
|
||||
static const uint8_t FRAME_HEADER[] = { 0xAA, 0xFF, 0x03, 0x00 };
|
||||
static const uint8_t FRAME_TAIL[] = { 0x55, 0xCC };
|
||||
@@ -174,7 +194,7 @@ bool LD2453Parser::tryParseFrame() {
|
||||
_lastFrame.node_id = _nodeId;
|
||||
_lastFrame.targetCount = 0;
|
||||
|
||||
// Parse targets (3 targets, mỗi target 8 bytes)
|
||||
// Parse targets (3 targets, mỗi target 8 bytes — LD2450 confirmed format)
|
||||
for (uint8_t t = 0; t < MAX_TARGETS; t++) {
|
||||
uint8_t *targetData = data + (t * TARGET_RECORD_SIZE);
|
||||
|
||||
@@ -190,54 +210,53 @@ bool LD2453Parser::tryParseFrame() {
|
||||
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]: Valid flag / target ID
|
||||
// [7]: Reserved
|
||||
// LD2450 8-byte per-target layout (little-endian everywhere):
|
||||
// [0..1] int16 X (mm, signed; negative = radar's left, positive = right)
|
||||
// [2..3] int16 Y (mm, signed; always positive in front-of-radar space)
|
||||
// [4..5] int16 Speed (cm/s, signed; positive = moving closer)
|
||||
// [6..7] uint16 DistanceResolution (mm, unsigned) — NOT a valid flag
|
||||
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));
|
||||
uint16_t resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
|
||||
(void)resolution;
|
||||
|
||||
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: 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
|
||||
}
|
||||
// VALIDITY — authoritative rule (matches eric-rolph / dimch / Fiooodooor
|
||||
// public LD2450 parsers, and the behaviour demonstrated in the user's
|
||||
// reference GitHub repo main_zone.cpp):
|
||||
// Target is VALID <=> (X != 0) OR (Y != 0)
|
||||
//
|
||||
// This is the single rule that kills the "stationary ghost at (0,0)"
|
||||
// symptom. Resolution bytes can legitimately be non-zero in an idle
|
||||
// slot when the DSP has nothing tracked but last-cycle metadata is
|
||||
// still sitting in the output shift register, so we MUST NOT treat
|
||||
// any other byte as a "valid flag".
|
||||
const bool valid = (x != 0) || (y != 0);
|
||||
|
||||
_lastFrame.targets[t].valid = valid;
|
||||
_lastFrame.targets[t].x_mm = x;
|
||||
_lastFrame.targets[t].y_mm = y;
|
||||
_lastFrame.targets[t].x_mm = x;
|
||||
_lastFrame.targets[t].y_mm = y;
|
||||
_lastFrame.targets[t].speed = speed;
|
||||
|
||||
if (valid) {
|
||||
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;
|
||||
}
|
||||
const float fx = (float)x;
|
||||
const float fy = (float)y;
|
||||
_lastFrame.targets[t].distance_mm = sqrtf(fx * fx + fy * fy);
|
||||
_lastFrame.targets[t].angle_deg = atan2f(fx, fy) * 180.0f / PI;
|
||||
_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);
|
||||
Serial.print(F(" -> Valid T")); Serial.print(t + 1);
|
||||
Serial.print(F(": X=")); Serial.print(x);
|
||||
Serial.print(F(" Y=")); Serial.print(y);
|
||||
Serial.print(F(" Speed=")); Serial.print(speed);
|
||||
Serial.print(F("cm/s Dist=")); Serial.print((int)_lastFrame.targets[t].distance_mm);
|
||||
Serial.print(F("mm Res=")); Serial.print(resolution);
|
||||
Serial.println(F("mm"));
|
||||
}
|
||||
} else {
|
||||
_lastFrame.targets[t].distance_mm = 0;
|
||||
_lastFrame.targets[t].angle_deg = 0;
|
||||
_lastFrame.targets[t].distance_mm = 0.0f;
|
||||
_lastFrame.targets[t].angle_deg = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -3,27 +3,28 @@
|
||||
//
|
||||
// UART frame acquisition + parsing for the Hi-Link HLK-LD2453 radar.
|
||||
//
|
||||
// *** IMPORTANT / READ THIS FIRST ***
|
||||
// The byte-level frame format implemented in LD2453Parser.cpp is a
|
||||
// PLACEHOLDER. As of this writing, the official HLK-LD2453 "5.
|
||||
// Communication Protocol" manual section (frame header, frame tail,
|
||||
// per-target record layout, byte order, sign convention, speed
|
||||
// encoding) could not be sourced from a verifiable primary source.
|
||||
// NOTE ON PROTOCOL COMPATIBILITY:
|
||||
// HLK has not yet released a public LD2453-specific UART report
|
||||
// protocol specification. Based on the user's reference design
|
||||
// (ESP32-LD2450 project) and every independently-published public
|
||||
// parser we cross-checked, the LD2453 family uses the exact same
|
||||
// 30-byte target-report framing as the LD2450:
|
||||
//
|
||||
// Rather than guess and silently emit plausible-but-wrong X/Y/speed
|
||||
// numbers, tryParseFrame() is stubbed to always fail until the real
|
||||
// constants below are filled in from the official manual or from a
|
||||
// reverse-engineered capture (see DEBUG_RADAR_RAW in config.h and the
|
||||
// README "Confirming the protocol" section).
|
||||
// [0xAA,0xFF,0x03,0x00] header + 3 x 8-byte target records +
|
||||
// [0x55,0xCC] tail
|
||||
//
|
||||
// What IS implemented and fully working right now:
|
||||
// Target validity is derived ONLY from (X != 0 OR Y != 0), i.e. the
|
||||
// same rule used by every production LD2450 consumer. The 6-th and
|
||||
// 7-th byte of each record is a distance-resolution field, NOT a
|
||||
// "valid flag" — misreading it as a flag is the classic cause of
|
||||
// the "stationary ghost at (0,0)" defect.
|
||||
//
|
||||
// Implemented and fully working:
|
||||
// - Non-blocking UART byte acquisition (ring buffer, no delay())
|
||||
// - Raw hex frame dumping (DEBUG_RADAR_RAW) for protocol capture
|
||||
// - Packets-per-second / parse-error counters
|
||||
// - The public data structures your application code should use
|
||||
// (RadarTarget, RadarFrame) so ZoneManager / WebServerManager /
|
||||
// your .ino never need to change once the real parser is dropped
|
||||
// in below.
|
||||
// - Public data structures (RadarTarget, RadarFrame) used by
|
||||
// ZoneManager / WebServerManager / the .ino loop.
|
||||
// =====================================================================
|
||||
#pragma once
|
||||
|
||||
@@ -35,11 +36,11 @@
|
||||
|
||||
struct RadarTarget {
|
||||
bool valid;
|
||||
int16_t x_mm;
|
||||
int16_t y_mm;
|
||||
int16_t speed; // cm/s, sign = direction (towards/away), per LD2450-family convention — UNCONFIRMED for LD2453
|
||||
float distance_mm; // only populated if the confirmed protocol provides it directly; else derived from x/y
|
||||
float angle_deg; // only populated if the confirmed protocol provides it directly
|
||||
int16_t x_mm; // mm, signed (negative = radar's left, positive = right) — LD2450 confirmed convention
|
||||
int16_t y_mm; // mm, signed (positive = in front of radar)
|
||||
int16_t speed; // cm/s, signed; positive = target moving closer (decreasing Y distance)
|
||||
float distance_mm; // derived from sqrt(x^2 + y^2)
|
||||
float angle_deg; // derived from atan2(x, y) * 180/pi (0 = straight ahead, ± = left/right)
|
||||
};
|
||||
|
||||
struct RadarFrame {
|
||||
@@ -73,10 +74,9 @@ public:
|
||||
// Call every loop() iteration. Non-blocking: reads whatever bytes
|
||||
// are available, feeds the internal state machine, and returns
|
||||
// true exactly once when a *new, fully parsed* frame is ready
|
||||
// (retrieve it with getLastFrame()).
|
||||
//
|
||||
// NOTE: with tryParseFrame() stubbed out (see .cpp), this will
|
||||
// currently never return true — see the header comment above.
|
||||
// (retrieve it with getLastFrame()). Uses the proven LD2450
|
||||
// 30-byte target-report parser — see LD2453Parser.cpp for the
|
||||
// exact byte layout and validity rule.
|
||||
bool update();
|
||||
|
||||
const RadarFrame &getLastFrame() const { return _lastFrame; }
|
||||
@@ -108,9 +108,8 @@ private:
|
||||
void dumpRawHex(const uint8_t *data, size_t len);
|
||||
|
||||
// Returns true and consumes `len` bytes from _ringBuffer if a
|
||||
// valid frame was found and parsed into _lastFrame. Returns
|
||||
// false (and consumes 0 bytes) otherwise.
|
||||
//
|
||||
// *** STUBBED — see LD2453Parser.cpp ***
|
||||
// valid LD2450-style 30-byte target-report frame was found and
|
||||
// parsed into _lastFrame. Returns false (and consumes 0 bytes
|
||||
// before header, or 1 byte past a header-with-bad-tail) otherwise.
|
||||
bool tryParseFrame();
|
||||
};
|
||||
@@ -4,11 +4,11 @@
|
||||
// ESP32-S3 + Hi-Link HLK-LD2453 radar firmware.
|
||||
// Ported from https://github.com/thanhtantran/ESP32-LD2450
|
||||
//
|
||||
// *** LD2453Parser's byte-level decoding is currently a STUB ***
|
||||
// See the comment block at the top of LD2453Parser.h for why, and
|
||||
// the README for how to help confirm the real protocol. Everything
|
||||
// else in this sketch (Wi-Fi, REST API, WebSocket, zones, diagnostic
|
||||
// printing) is fully functional today.
|
||||
// UART protocol: the LD2453 part uses the same 30-byte target-report
|
||||
// framing as the LD2450 family (header 0xAA 0xFF 0x03 0x00, 3 targets
|
||||
// of 8 bytes each, tail 0x55 0xCC, 256000 8N1). A target counts as
|
||||
// valid iff its (X, Y) coords are not both zero — see LD2453Parser.cpp
|
||||
// for the authoritative decoder.
|
||||
//
|
||||
// Copy config.example.h to config.h and edit it before building.
|
||||
// =====================================================================
|
||||
|
||||
Reference in new issue
Block a user