lc-smartX/lc/radar_event.go

package lc

import (
	"github.com/jacobsa/go-serial/serial"
	"github.com/sirupsen/logrus"
	"lc-smartX/lc/model"
	"lc-smartX/util"
	"math"
	"strconv"
	"time"
)

// 雷达数据结构体（存储解析后的有效数据）
type RadarData struct {
	Direction string  // 方向：in(来向)/out(去向)
	Speed     float64 // 速度值（单位：根据雷达手册，如km/h）
	Valid     bool    // 数据是否有效
}

// 单雷达服务器
func NewRadarEventServer() *RadarServer {
	// 只取第一个雷达配置（单个设备）
	radar := util.Config.Radars[0]
	s := &RadarServer{
		Radar:    radar,
		Notifier: nil, // 单个回调
	}
	return s
}

type RadarServer struct {
	Radar    model.RadarInfo // 单个雷达信息
	Notifier Notifier        // 单个回调
}

// 启动单个雷达串口监听
func (s *RadarServer) Start() {
	go s.OpenSerial(s.Radar.Port) // 直接启动当前雷达
}

// 注册回调（无需分支判断）
func (s *RadarServer) RegisterCallback(notifier Notifier) {
	s.Notifier = notifier
}

// 兼容原空参数Callback方法（保持接口兼容）
func (s *RadarServer) Callback() {
	s.CallbackWithData(RadarData{
		Direction: "unknown",
		Speed:     0,
		Valid:     false,
	})
}

// 带数据的回调方法（核心回调逻辑）
func (s *RadarServer) CallbackWithData(radarData RadarData) {
	if s.Notifier == nil {
		logrus.Error("雷达回调未注册")
		return
	}
	// 自定义触发条件：速度>0时触发警告（可根据业务调整阈值，如>5）
	if radarData.Valid && radarData.Speed > float64(DeviceInitData.LowSpeed) {
		// 拼接方向和速度作为通知消息
		s.Notifier.Notify("主路来车", 0, int(math.Round(radarData.Speed)))
	}
}

// 打开串口（树莓派USB转485）+ 解析9字节雷达协议
func (s *RadarServer) OpenSerial(portName string) {
	// 串口配置（严格匹配雷达手册：9600bps、8数据位、1停止位、无校验）
	options := serial.OpenOptions{
		PortName:        portName, // 树莓派USB转485通常为/dev/ttyUSB0
		BaudRate:        9600,     // 根据雷达手册调整波特率
		DataBits:        8,
		StopBits:        1,
		MinimumReadSize: 9,                  // 每帧固定9字节，设置最小读取长度
		ParityMode:      serial.PARITY_NONE, // 无校验（根据雷达手册调整）
	}

	// 打开串口
	port, err := serial.Open(options)
	if err != nil {
		logrus.Fatalf("雷达串口打开失败: %v", err)
	}
	defer port.Close()
	//logrus.Infof("雷达串口 %s 已打开，开始解析9字节V+/V-协议数据", portName)

	// ======================== 新增：发送雷达配置指令 ========================
	// 配置指令：来向探测、≈4帧/秒（0x03）、千米/小时（0x00）
	radarCmd := []byte{0x43, 0x46, 0x02, 0x01, 0x03, 0x00, 0x0d, 0x0a}

	// 发送指令
	sendLen, err := port.Write(radarCmd)
	if err != nil {
		logrus.Errorf("雷达配置指令发送失败: %v", err)
	} else if sendLen != len(radarCmd) {
		logrus.Warnf("雷达配置指令发送不完整，发送%d字节，预期%d字节", sendLen, len(radarCmd))
	} else {
		logrus.Infof("雷达配置指令发送成功，共发送%d字节", sendLen)
	}

	// 延迟100ms，确保指令完全发送并被雷达接收（根据雷达响应调整）
	time.Sleep(100 * time.Millisecond)
	// ======================== 新增结束 ========================

	// 缓存读取的字节（处理可能的粘包/拆包）
	readBuf := make([]byte, 0, 32)
	buf := make([]byte, 9) // 单次读取9字节

	// 持续读取雷达数据
	for {
		// 读取9字节数据
		n, err := port.Read(buf)
		if err != nil {
			logrus.Errorf("雷达读取错误: %v，3秒后尝试重连", err)
			time.Sleep(3 * time.Second)
			s.OpenSerial(portName) // 重连串口
			return
		}

		// 拼接读取到的字节（处理拆包）
		readBuf = append(readBuf, buf[:n]...)

		// 确保缓存中有完整的9字节帧才解析
		for len(readBuf) > 0 {
			// 查找帧分隔符（\r 或 \n），定位帧结束位置
			frameEndIdx := -1
			for i, b := range readBuf {
				if b == 0x0D || b == 0x0A {
					frameEndIdx = i
					break
				}
			}
			if frameEndIdx == -1 {
				// 无分隔符，若缓存超过32字节则清空（防止垃圾数据堆积）
				if len(readBuf) > 32 {
					readBuf = readBuf[len(readBuf)-16:] // 保留最后16字节，避免丢失有效数据
				}
				break
			}

			// 提取一帧数据（从开头到分隔符）
			frame := readBuf[:frameEndIdx+1]
			readBuf = readBuf[frameEndIdx+1:] // 剩余数据留到下次解析

			// 解析帧
			radarData := s.parseRadarFrame(frame)
			if radarData.Valid {
				//logrus.Debugf("解析到有效雷达数据：方向=%s，速度=%.1f", radarData.Direction, radarData.Speed)
				// 触发带数据的回调
				s.CallbackWithData(radarData)
			} else {
				// 无效帧仅打印调试日志（不触发回调）
				//logrus.Debugf("无效雷达帧，原始字节：%x", frame)
			}
		}
	}
}

// 解析单帧雷达数据（优化：兼容前缀空白、帧尾不完整）
func (s *RadarServer) parseRadarFrame(frame []byte) RadarData {
	var data RadarData

	// ========== 新增：预处理帧数据 - 清理前缀空白字符（\n/\r/空格/制表符） ==========
	// 定义需要清理的空白字符：0x00(空)、0x0a(\n)、0x0d(\r)、0x20(空格)、0x09(制表符)
	trimPrefixChars := []byte{0x00, 0x0a, 0x0d, 0x20, 0x09}
	// 循环清理开头的空白字符
	trimmedFrame := frame
	for len(trimmedFrame) > 0 {
		isTrimChar := false
		for _, c := range trimPrefixChars {
			if trimmedFrame[0] == c {
				trimmedFrame = trimmedFrame[1:] // 移除首字符
				isTrimChar = true
				break
			}
		}
		if !isTrimChar {
			break // 无空白字符，停止清理
		}
	}
	// 调试日志：打印预处理前后的帧数据
	//logrus.Debugf("帧预处理：原始=%q (0x%x) → 清理后=%q (0x%x)", frame, frame, trimmedFrame, trimmedFrame)

	// ========== 1. 基础校验：清理后的数据至少保留核心7字节（V+001.9） ==========
	if len(trimmedFrame) < 7 {
		//logrus.Warnf("雷达帧长度异常，清理后不足7字节，实际%d字节", len(trimmedFrame))
		return data
	}

	// ========== 2. 校验帧头（首字节必须是ASCII的'V'） ==========
	if trimmedFrame[0] != 'V' {
		//logrus.Debugf("雷达帧头错误，期望'V'(0x56)，实际0x%02x（字符：%q）", trimmedFrame[0], trimmedFrame[0])
		return data
	}

	// ========== 3. 适配帧尾：兼容仅含\r（0x0D）或完整\r\n（0x0D+0x0A） ==========
	// 核心数据长度：V + +/- + 百位+十位+个位 + . + 小数位 = 7字节（索引0-6）
	// 帧尾允许两种情况：7字节后是\r（0x0D） 或 \r\n（0x0D+0x0A）
	var frameTailValid bool
	if len(trimmedFrame) >= 8 && trimmedFrame[7] == 0x0D {
		// 情况1：7字节核心数据 + \r（共8字节）
		frameTailValid = true
	} else if len(trimmedFrame) >= 9 && trimmedFrame[7] == 0x0D && trimmedFrame[8] == 0x0A {
		// 情况2：7字节核心数据 + \r\n（共9字节，标准协议）
		frameTailValid = true
	}
	if !frameTailValid {
		//logrus.Debugf("雷达帧尾错误，清理后帧尾：%q (0x%x)，期望\\r 或 \\r\\n", trimmedFrame[7:], trimmedFrame[7:])
		return data
	}

	// ========== 4. 解析方向（第2字节：+ 来向 / - 去向） ==========
	switch trimmedFrame[1] {
	case '+':
		data.Direction = "in" // 来向目标
	case '-':
		data.Direction = "out" // 去向目标
	default:
		//logrus.Debugf("雷达方向标识错误，期望'+'/'-'，实际0x%02x（字符：%q）", trimmedFrame[1], trimmedFrame[1])
		return data
	}

	// ========== 5. 校验小数点位置（第6字节必须是'.'） ==========
	if trimmedFrame[5] != '.' {
		//logrus.Debugf("雷达小数点位置错误，期望0x2E('.')，实际0x%02x（字符：%q）", trimmedFrame[5], trimmedFrame[5])
		return data
	}

	// ========== 6. 字符转数字（ASCII字符转数值，如'1'→1） ==========
	// 百位：trimmedFrame[2] → 0-9的ASCII字符
	hundreds, err := strconv.Atoi(string(trimmedFrame[2]))
	if err != nil {
		//logrus.Debugf("雷达百位数值错误：%v（字符：%q）", err, trimmedFrame[2])
		return data
	}
	// 十位：trimmedFrame[3]
	tens, err := strconv.Atoi(string(trimmedFrame[3]))
	if err != nil {
		//logrus.Debugf("雷达十位数值错误：%v（字符：%q）", err, trimmedFrame[3])
		return data
	}
	// 个位：trimmedFrame[4]
	units, err := strconv.Atoi(string(trimmedFrame[4]))
	if err != nil {
		//logrus.Debugf("雷达个位数值错误：%v（字符：%q）", err, trimmedFrame[4])
		return data
	}
	// 小数位：trimmedFrame[6]
	decimal, err := strconv.Atoi(string(trimmedFrame[6]))
	if err != nil {
		//logrus.Debugf("雷达小数位数值错误：%v（字符：%q）", err, trimmedFrame[6])
		return data
	}

	// ========== 7. 计算最终速度值（xxx.x格式） ==========
	data.Speed = float64(hundreds*100+tens*10+units) + float64(decimal)/10.0
	// 标记数据有效
	data.Valid = true

	//logrus.Debugf("解析成功：方向=%s，速度=%.1f（原始数据：%q）", data.Direction, data.Speed, frame)
	return data
}