HLK-LD6004 - 60GHz Radar Monitor - Test Script

ld6004_monitor.py

#!/usr/bin/env python3
"""HLK-LD6004 / LD6002B TinyFrame radar monitor.

Usage: python3 ld6004_monitor.py [/dev/ttyUSB0]
"""

import os
import sys
import struct
import time
import serial

BAUD = 115200
SOF = 0x01

# Message types
TYPE_TARGET   = 0x0A04
TYPE_CLOUD    = 0x0A08
TYPE_PRESENCE = 0x0A0A
TYPE_BOOT     = 0x0100


def checksum(data: bytes) -> int:
    r = 0
    for b in data:
        r ^= b
    return (~r) & 0xFF


def parse_header(buf: bytes):
    """Parse 8-byte TinyFrame header. Returns (id, length, type) or None."""
    if len(buf) < 8 or buf[0] != SOF:
        return None
    frame_id = struct.unpack(">H", buf[1:3])[0]
    data_len = struct.unpack(">H", buf[3:5])[0]
    msg_type = struct.unpack(">H", buf[5:7])[0]
    head_ck  = buf[7]
    if head_ck != checksum(buf[0:7]):
        return None
    if data_len > 1024:
        return None
    return frame_id, data_len, msg_type


def parse_targets(data: bytes):
    """Parse TARGET_LOCATION payload. Returns (count, list of (x, y, z, dop, cid))."""
    if len(data) < 4:
        return 0, []
    n = struct.unpack("<I", data[0:4])[0]
    targets = []
    for i in range(min(n, 3)):
        off = 4 + i * 20
        if off + 20 > len(data):
            break
        x   = struct.unpack("<f", data[off:off+4])[0]
        y   = struct.unpack("<f", data[off+4:off+8])[0]
        z   = struct.unpack("<f", data[off+8:off+12])[0]
        dop = struct.unpack("<i", data[off+12:off+16])[0]
        cid = struct.unpack("<i", data[off+16:off+20])[0]
        targets.append((x, y, z, dop, cid))
    return n, targets


def parse_presence(data: bytes):
    """Parse PRESENCE_STATE payload. Returns list of 4 area states."""
    if len(data) < 16:
        return []
    return [struct.unpack("<I", data[i:i+4])[0] for i in range(0, 16, 4)]


def render(areas, target_count, targets, fps):
    """Render the current state to the terminal using ANSI escape codes."""
    # Move cursor home and clear
    sys.stdout.write("\033[H\033[J")

    print("╔══════════════════════════════════════════════════════════════╗")
    print("║                HLK-LD6004  60GHz Radar Monitor               ║")
    print("╠══════════════════════════════════════════════════════════════╣")

    # Presence
    zone_strs = []
    for i, a in enumerate(areas):
        if a:
            zone_strs.append(f" \033[1;32m■ Zone {i}\033[0m")
        else:
            zone_strs.append(f" \033[2m□ Zone {i}\033[0m")
    print(f"║  Presence: {' '.join(zone_strs):<50}           ║")

    print("╠══════════════════════════════════════════════════════════════╣")
    print(f"║  Targets: {target_count:<51}║")
    print("║                                                              ║")

    # Header
    print("║   #  cid      X        Y        Z      dop                   ║")
    print("║  ─── ───  ────────  ────────  ────────  ───                  ║")

    for i in range(3):
        if i < len(targets):
            x, y, z, dop, cid = targets[i]
            # Color dop: green=approaching(+), red=receding(-), dim=0
            if dop > 0:
                dop_str = f"\033[32m{dop:+3d}\033[0m"
            elif dop < 0:
                dop_str = f"\033[31m{dop:+3d}\033[0m"
            else:
                dop_str = f"\033[2m  0\033[0m"
            print(f"║  T{i+1}  {cid:>3}  {x:+8.3f}  {y:+8.3f}  {z:+8.3f}  {dop_str}                  ║")
        else:
            print(f"║  T{i+1}  \033[2m ---     ---       ---       ---   ---\033[0m                  ║")

    print("║                                                              ║")
    print("╠══════════════════════════════════════════════════════════════╣")
    print(f"║  {fps:4.1f} fps                                Ctrl+C to quit      ║")
    print("╚══════════════════════════════════════════════════════════════╝")

    sys.stdout.flush()


def main():
    port = sys.argv[1] if len(sys.argv) > 1 else "/dev/ttyUSB0"

    ser = serial.Serial(port, BAUD, timeout=1)
    ser.reset_input_buffer()

    # Hide cursor
    sys.stdout.write("\033[?25l")
    sys.stdout.flush()

    buf = bytearray()
    areas = [0, 0, 0, 0]
    target_count = 0
    targets = []
    frame_count = 0
    fps = 0.0
    last_fps_time = time.monotonic()
    dirty = False

    try:
        # Initial draw
        render(areas, target_count, targets, fps)

        while True:
            chunk = ser.read(ser.in_waiting or 1)
            if not chunk:
                continue
            buf.extend(chunk)

            while True:
                idx = buf.find(bytes([SOF]))
                if idx < 0:
                    buf.clear()
                    break
                if idx > 0:
                    buf = buf[idx:]

                if len(buf) < 8:
                    break

                hdr = parse_header(bytes(buf[:8]))
                if hdr is None:
                    buf.pop(0)
                    continue

                frame_id, data_len, msg_type = hdr
                frame_size = 8 + data_len + 1

                if len(buf) < frame_size:
                    break

                payload = bytes(buf[8:8 + data_len])
                data_ck = buf[8 + data_len]
                expected = checksum(payload) if data_len > 0 else 0xFF

                buf = buf[frame_size:]

                if data_ck != expected:
                    continue

                if msg_type == TYPE_BOOT:
                    continue

                if msg_type == TYPE_PRESENCE:
                    areas = parse_presence(payload) or areas
                    dirty = True

                elif msg_type == TYPE_TARGET:
                    target_count, targets = parse_targets(payload)
                    frame_count += 1
                    dirty = True

            # Update display after processing all buffered messages
            if dirty:
                now = time.monotonic()
                elapsed = now - last_fps_time
                if elapsed >= 1.0:
                    fps = frame_count / elapsed
                    frame_count = 0
                    last_fps_time = now
                render(areas, target_count, targets, fps)
                dirty = False

    except KeyboardInterrupt:
        pass
    finally:
        # Show cursor, clear screen
        sys.stdout.write("\033[?25h\n")
        sys.stdout.flush()
        ser.close()
        print("Done.")


if __name__ == "__main__":
    main()