license_plate_recognition/modules/radar/radar.py

import math
import time
from typing import List, Optional

import serial

from modules.audio.speaker import IpCast
from modules.display.screen import Screen, FlashFile


class RadarData:
    """对应Go的RadarData结构体"""

    def __init__(self):
        self.Valid: bool = False  # 数据是否有效
        self.Direction: str = ""  # 方向：in(来向)/out(去向)
        self.Speed: float = 0.0  # 速度（xxx.x格式）


class DeviceInitData:
    """模拟Go的DeviceInitData，存储速度阈值（需根据实际业务赋值）"""
    LowSpeed: float = 0.0  # 速度阈值，比如5.0（速度>5触发回调）


# ========== 新增1：超时清空状态管理 ==========
class ScreenClearManager:
    """屏幕超时清空管理器"""

    def __init__(self, timeout_seconds: float = 2.0):
        self.timeout_seconds = timeout_seconds  # 超时时间（秒）
        self.last_valid_time = 0.0  # 最后一次收到有效数据的时间
        self.screen_cleared = False  # 屏幕是否已清空

    def update_valid_time(self):
        """更新有效数据时间"""
        self.last_valid_time = time.time()
        self.screen_cleared = False

    def check_and_clear(self, radar_screen: Optional[Screen]) -> bool:
        """
        检查是否需要清空屏幕
        :return: 是否清空了屏幕
        """
        if radar_screen is None or not radar_screen.get_live_state():
            return False

        current_time = time.time()
        time_since_last_valid = current_time - self.last_valid_time

        if time_since_last_valid > self.timeout_seconds and not self.screen_cleared:
            # 超时了，清空屏幕
            print(f"⏱️  雷达超时：{time_since_last_valid:.1f}秒无有效数据，清空屏幕 {radar_screen.conn.getpeername()}")
            try:
                ff = FlashFile()
                ff.set_msg("00", 0)  # 发送空内容
                ff.set_mode(4, 2)
                ff.set_origin(0, True, 0)
                ff.set_area(128, True, 96)
                radar_screen.text_ram(ff, False)
                self.screen_cleared = True
                return True
            except Exception as e:
                print(f"❌ 清空屏幕失败：{e}")

        return False


# 初始化全局的超时清空管理器
screen_clear_manager = ScreenClearManager(timeout_seconds=2.0)


# ========== 核心：解析单帧雷达数据（完全复刻原Go逻辑） ==========
def parse_radar_frame(frame: bytes) -> RadarData:
    """
    解析单帧雷达数据（兼容前缀空白、帧尾不完整）
    :param frame: 原始帧字节数据
    :return: 解析后的RadarData对象
    """
    data = RadarData()

    # ========== 1. 预处理帧数据：清理前缀空白字符 ==========
    trim_prefix_chars = {0x00, 0x0A, 0x0D, 0x20, 0x09}
    trimmed_frame = bytearray(frame)

    while len(trimmed_frame) > 0:
        if trimmed_frame[0] in trim_prefix_chars:
            trimmed_frame.pop(0)
        else:
            break

    # ========== 2. 基础校验：清理后至少保留7字节（V+001.9） ==========
    if len(trimmed_frame) < 7:
        return data

    # ========== 3. 校验帧头（首字节必须是ASCII的'V'，0x56） ==========
    if trimmed_frame[0] != ord('V'):
        return data

    # ========== 4. 适配帧尾：兼容仅含\r（0x0D）或完整\r\n（0x0D+0x0A） ==========
    frame_tail_valid = False
    if len(trimmed_frame) >= 8 and trimmed_frame[7] == 0x0D:
        frame_tail_valid = True
    elif len(trimmed_frame) >= 9 and trimmed_frame[7] == 0x0D and trimmed_frame[8] == 0x0A:
        frame_tail_valid = True

    if not frame_tail_valid:
        return data

    # ========== 5. 解析方向（第2字节：+ 来向/in / - 去向/out） ==========
    direction_char = chr(trimmed_frame[1])
    if direction_char == '+':
        data.Direction = "in"
    elif direction_char == '-':
        data.Direction = "out"
    else:
        return data

    # ========== 6. 校验小数点位置（第6字节必须是'.'，0x2E） ==========
    if trimmed_frame[5] != ord('.'):
        return data

    # ========== 7. ASCII字符转数字（如'1'→1） ==========
    try:
        hundreds = int(chr(trimmed_frame[2]))
        tens = int(chr(trimmed_frame[3]))
        units = int(chr(trimmed_frame[4]))
        decimal = int(chr(trimmed_frame[6]))
    except ValueError:
        return data

    # ========== 8. 计算最终速度值（xxx.x格式） ==========
    data.Speed = float(hundreds * 100 + tens * 10 + units) + float(decimal) / 10.0
    data.Valid = True

    # ========== 新增2：打印解析到的原始数据（调试用） ==========
    print(f"📡 雷达解析成功：方向={data.Direction}，速度={data.Speed:.1f}，原始帧={frame!r}")

    return data


def callback_with_data(
        radar_data: RadarData,
        radar_serial=None,
        speaker: Optional[IpCast] = None,
        radar_screen: Optional[Screen] = None
):
    """
    雷达数据回调（新增超时清空逻辑）
    """
    if radar_serial is None:
        import detect_plate
        radar_serial = detect_plate.radar_serial

    # 速度阈值判断（触发条件）
    if radar_data.Valid and radar_data.Speed > DeviceInitData.LowSpeed:
        color = 1
        rounded_speed = str(round(radar_data.Speed))

        if radar_data.Speed < 20:
            color = 2

        speak_text = f"注意来车"

        # 语音播报
        if speaker:
            try:
                speaker.Speak(speak_text)
            except Exception as e:
                print(f"语音播报失败：{e}")

        # 屏幕显示
        if radar_screen and radar_screen.get_live_state():
            try:
                ff = FlashFile()
                ff.set_msg(rounded_speed, color)
                ff.set_mode(4, 2)
                ff.set_origin(0, True, 0)
                ff.set_area(128, True, 96)
                radar_screen.text_ram(ff, False)

                # ========== 新增3：更新有效数据时间 ==========
                screen_clear_manager.update_valid_time()

                print(f"✅ 雷达速度{rounded_speed}已发送到屏幕")
            except Exception as e:
                print(f"雷达屏幕显示失败：{e}")


def open_serial(
        port_name: str,
        speaker: Optional[IpCast] = None,
        radar_screen: Optional[Screen] = None,
        radar_serial_ref=None
):
    """打开雷达串口并监听（彻底解决残留数据问题）"""
    radar_serial = radar_serial_ref

    serial_options = {
        "port": port_name,
        "baudrate": 9600,
        "bytesize": serial.EIGHTBITS,
        "stopbits": serial.STOPBITS_ONE,
        "parity": serial.PARITY_NONE,
        "timeout": 0.1,
    }

    while True:
        try:
            radar_serial = serial.Serial(**serial_options)
            print(f"雷达串口 {port_name} 已打开")

            # 发送雷达配置指令
            radar_cmd = bytes([0x43, 0x46, 0x02, 0x01, 0x03, 0x00, 0x0d, 0x0a])
            send_len = radar_serial.write(radar_cmd)
            if send_len != len(radar_cmd):
                print(f"雷达配置指令发送不完整")
            time.sleep(0.1)

            read_buf: List[int] = []
            buf = bytearray(1024)
            last_clear_check = time.time()
            last_valid_data_time = time.time()

            while True:
                try:
                    n = radar_serial.readinto(buf)

                    # 定期检查超时清空
                    current_time = time.time()
                    if current_time - last_clear_check > 0.2:
                        screen_clear_manager.check_and_clear(radar_screen)
                        last_clear_check = current_time

                    if n == 0:
                        continue

                    read_buf.extend(buf[:n])

                    while len(read_buf) > 0:
                        frame_end_idx = -1
                        for i, b in enumerate(read_buf):
                            if b == 0x0D or b == 0x0A:
                                frame_end_idx = i
                                break
                        if frame_end_idx == -1:
                            if len(read_buf) > 32:
                                read_buf = read_buf[-16:]
                            break

                        # 解析雷达帧
                        frame = bytes(read_buf[:frame_end_idx + 1])
                        read_buf = read_buf[frame_end_idx + 1:]
                        radar_data = parse_radar_frame(frame)

                        if radar_data.Valid:
                            data_time = time.time()
                            time_since_last = data_time - last_valid_data_time

                            # 更新时间
                            last_valid_data_time = data_time
                            last_clear_check = current_time

                            # 日志显示
                            # local_time = time.strftime("%H:%M:%S", time.localtime(data_time))
                            # print(f"📡 [{local_time}] 速度={radar_data.Speed:.1f} (距上次{time_since_last:.2f}秒)")
                            if radar_data.Speed < 5:
                                continue

                            # 触发回调
                            callback_with_data(radar_data, radar_serial, speaker, radar_screen)

                            read_buf = []
                            break  # 退出当前的帧处理循环

                except Exception as read_err:
                    print(f"雷达读取错误：{read_err}，3秒后重连")
                    time.sleep(3)
                    break
        except Exception as e:
            print(f"雷达串口打开失败：{e}，3秒后重试")
            time.sleep(3)
        finally:
            if 'radar_serial' in locals() and radar_serial and radar_serial.is_open:
                radar_serial.close()
                print(f"雷达串口 {port_name} 已关闭")