fix lỗi giải mã byte encoding

esp32s3-ld2453/LD2453Parser.cpp

@@ -16,21 +16,44 @@
 // ---------------------------------------------------------------
 // LD2450 / LD2453 SERIAL FRAMING — PER-USER-DIRECTED POLICY.
 //
+// (0) CRITICAL DIFFERENCE LD2450 vs LD2453 (ROOT CAUSE of "hand wave
+//     still invalid" reported by the user):
+//     * LD2450 coordinate encoding per slot  = standard 2's complement
+//                                              signed int16 little-endian.
+//     * LD2453 coordinate encoding per slot  = 16-bit SIGN-MAGNITUDE,
+//                                              little-endian byte order.
+//         bit 15 (MSB) = sign flag.
+//             1 ⇒ POSITIVE.
+//             0 ⇒ NEGATIVE.
+//         bits 14..0     = 15-bit unsigned magnitude (absolute value).
+//
+//     Evidence from the user's 2026-09-13 hand-wave capture:
+//       Raw slot-1 bytes = [3B 80 BF 80 00 00 4A 01]
+//       Previous (LD2450-style) 2's-complement cast → X=-32709 Y=-32577
+//           → false "implausible WARN" even on a real target.
+//       New LD2453 sign-magnitude decode:
+//           X =  0x803B → sign=+  magnitude=0x003B =  59 mm
+//           Y =  0x80BF → sign=+  magnitude=0x00BF = 191 mm
+//           → distance ≈ 200 mm ≈ hand 20 cm in front of the antenna,
+//             perfectly plausible → VALID ✔
+//
+// Speed (bytes 4-5 of each slot) is still interpreted as standard
+// signed int16 LE (cm/s); distance-resolution (bytes 6-7) stays as
+// standard uint16 LE (mm).  Speed and resolution encodings are
+// IDENTICAL between LD2450 and LD2453.
+//
 // The user explicitly asked for the following behaviour — we follow it
 // exactly below and do NOT invent anything else:
 //   (a) NEVER reject a syntactically-valid 30-byte 0x0003 frame as a
 //       whole.  Every [AA FF 03 00 .. .. 55 CC] gets counted in
 //       framesParsed and promoted to _lastFrame.
-//   (b) Per-slot sanity is the ONLY filtering point.  If a slot's decoded
+//   (b) Per-slot sanity is the ONLY filtering point.  If a slot's
-//       coordinates are physically implausible (Y < 0, Y > 6m,
+//       SIGN-MAGNITUDE decoded coords are physically implausible
-//       |X| > 6m, …):
+//       (Y <= 0, Y > ~6.5m, |X| > ~6.5m, …):
 //         * we print a WARN line to Serial Monitor,
 //         * mark the slot valid=false IN the promoted RadarFrame,
 //         * increment badSlotWarnings stat,
-//         * do NOT send it up to the WebSocket / OLED / UI.
+//         * do NOT expose it to WebSocket / OLED / UI.
-//
-// This "trust the wire unless proven-impossible per-slot" policy maximises
-// recall — zero whole-frame dropped data loss.
 //
 // (1) WIRE LAYOUT (byte-level, both chips):
 //     Sync 2 bytes  : 0xAA, 0xFF
@@ -40,16 +63,16 @@
 //
 // (2) ACCEPTED FRAME TYPES FOR PARSING:
 //     * 0x0003 — LD2450/LD2453 3-slot tracking report (30 bytes
-//                 total / 24 payload / 3 × 8 B/slot).  This is the only
+//                 total / 24 payload / 3 × 8 B/slot).  Only difference
-//                 format we know how to interpret today.  Frames with any
+//                 vs LD2450 = X/Y sign-magnitude encoding (see §0).
-//                 other frame-type words are skipped byte-by-byte until a
-//                 fresh AA FF sync is found.
 //
 // (3) 8-BYTE SLOT LAYOUT (0x0003 frame payload slots):
-//       [0..1]  int16  X_mm            signed; <0 left, >0 right
+//       [0..1]  X_mm          LD2453 = sign-magnitude uint16 LE
-//       [2..3]  int16  Y_mm            signed; ALWAYS >0 for a real person
+//                              LD2450 = 2's-compl signed int16 LE
-//       [4..5]  int16  Speed_cm_s      signed; +ve = closer
+//                              Use decodeCoordSM16() below (LD2453 rule).
-//       [6..7]  uint16 DistanceRes_mm  unsigned; NOT a "valid flag"
+//       [2..3]  Y_mm          SAME ENCODING AS X.  Y >= 1 for real target.
+//       [4..5]  Speed_cm_s    standard signed int16 LE, cm/s.
+//       [6..7]  DistRes_mm    standard uint16 LE, mm.
 // ---------------------------------------------------------------
 static const uint8_t FRAME_SYNC[] = { 0xAA, 0xFF };
 static const uint8_t FRAME_TAIL[] = { 0x55, 0xCC };
@@ -71,10 +94,23 @@ static const uint16_t accepted_target_types[] = {
 static const size_t accepted_target_types_count =
     sizeof(accepted_target_types) / sizeof(accepted_target_types[0]);
 
-// Per-slot physical plausibility bounds (used for the WARN step (b) above).
+/* LD2453 SIGN-MAGNITUDE coordinate decoder (§0 above).
+ * Takes the 2 little-endian slot bytes as [lo, hi] and returns a
+ * signed mm coordinate.  */
+static inline int32_t decodeCoordSM16(uint8_t lo, uint8_t hi) {
+    const uint16_t raw = (uint16_t)lo | ((uint16_t)hi << 8);
+    const bool positive = (raw & 0x8000u) != 0u; /* bit15 = sign flag */
+    const uint32_t magnitude = (uint32_t)(raw & 0x7FFFu); /* bits 0..14 */
+    const int32_t sign = positive ? 1 : -1;
+    return sign * (int32_t)magnitude;
+}
+
+// Per-slot physical plausibility bounds.  LD2453 nominal range = 0.5–6m,
+// use 6.5 m as an upper-bound ceiling so values just inside max range
+// (like the earlier ACK-like 6033 mm case) do not get false-WARNed.
 static const int32_t Y_MIN_MM     = 1;
-static const int32_t Y_MAX_MM     = 6000;
+static const int32_t Y_MAX_MM     = 6500;
-static const int32_t X_ABS_MAX_MM = 6000;
+static const int32_t X_ABS_MAX_MM = 6500;
 
 // Returns true iff decoded (x,y) sit inside the radar's measurable
 // operational envelope (in front of the antenna and inside rated
@@ -272,8 +308,8 @@ bool LD2453Parser::tryParseFrame() {
             }
         }
 
-        int16_t  x          = (int16_t)(targetData[0] | (targetData[1] << 8));
+        int32_t  x          = decodeCoordSM16(targetData[0], targetData[1]);
-        int16_t  y          = (int16_t)(targetData[2] | (targetData[3] << 8));
+        int32_t  y          = decodeCoordSM16(targetData[2], targetData[3]);
         int16_t  speed      = (int16_t)(targetData[4] | (targetData[5] << 8));
         uint16_t resolution = (uint16_t)(targetData[6] | (targetData[7] << 8));
         (void)resolution;

orangepi-server/visualizer.py

@@ -135,15 +135,13 @@ class RadarState:
         self.target_count = frame.get("target_count", 0)
         now = time.time()
 
-        # Belt-and-suspenders physical-limits filter.  The ESP32 side
+        # Belt-and-suspenders physical-limits filter, kept in sync with
-        # should already have gated these, but in case old firmware is
+        # the LD2453 ESP32 parser (sign-magnitude 15-bit magnitude +
-        # still running or an ACK frame leaks through, we re-apply the
+        # nominal range extended to 6.5 m ceiling to avoid false drops
-        # same sanity rules on the Orange Pi side:
+        # on real near-max-range targets, see LD2453Parser.cpp §0 and §2).
-        #   Y in front of radar (y >= 1 mm), <= 6000 mm rated range
-        #   |X| <= 6000 mm (±60° cone + safety margin)
         PHYS_Y_MIN = 1
-        PHYS_Y_MAX = 6000
+        PHYS_Y_MAX = 6500
-        PHYS_X_ABS = 6000
+        PHYS_X_ABS = 6500
         valid_targets_this_frame = 0
 
         targets_data = frame.get("targets", [])