377 lines
9.9 KiB
Go
377 lines
9.9 KiB
Go
package driver
|
||
|
||
import (
|
||
"errors"
|
||
"fmt"
|
||
"io"
|
||
"sync"
|
||
"time"
|
||
|
||
"github.com/tarm/serial"
|
||
)
|
||
|
||
const BufSize = 2048
|
||
|
||
type LCX6XZ struct {
|
||
NMEA_RMC *NMEA_RMC
|
||
NMEA_GGA *NMEA_GGA
|
||
NMEA_GLL *NMEA_GLL
|
||
NMEA_VTG *NMEA_VTG
|
||
NMEA_GSA *NMEA_GSA
|
||
NMEA_GSV *NMEA_GSV
|
||
OutputRates map[NMEA_SUB_ID]uint8 // 存储查询到的输出速率
|
||
ResData []byte
|
||
mutex sync.Mutex
|
||
uartAckCh chan struct{}
|
||
uartFd io.ReadWriteCloser
|
||
}
|
||
|
||
func UartRX_Task(lcx6xz *LCX6XZ) {
|
||
var dataBuffer []byte // 累积数据缓冲区
|
||
readBuffer := make([]byte, 1024) // 单次读取缓冲区
|
||
|
||
for {
|
||
// 读取串口数据
|
||
n, err := lcx6xz.uartFd.Read(readBuffer)
|
||
|
||
// 处理读取错误
|
||
if err != nil {
|
||
if err.Error() == "timeout" {
|
||
// 超时是正常的，继续读取
|
||
continue
|
||
}
|
||
fmt.Printf("串口读取错误: %v\n", err)
|
||
time.Sleep(100 * time.Millisecond) // 避免死循环
|
||
continue
|
||
}
|
||
|
||
// 没有读取到数据
|
||
if n == 0 {
|
||
time.Sleep(10 * time.Millisecond) // 避免CPU占用过高
|
||
continue
|
||
}
|
||
|
||
// 将新数据追加到累积缓冲区
|
||
dataBuffer = append(dataBuffer, readBuffer[:n]...)
|
||
|
||
// 处理累积缓冲区中的完整NMEA语句
|
||
dataBuffer = processNMEAData(dataBuffer, lcx6xz)
|
||
|
||
// 防止缓冲区无限增长
|
||
if len(dataBuffer) > BufSize {
|
||
fmt.Println("WARNING: 数据缓冲区过大，清空缓冲区")
|
||
dataBuffer = dataBuffer[:0] // 清空缓冲区
|
||
}
|
||
}
|
||
}
|
||
|
||
// processNMEAData 处理NMEA数据并返回剩余的未处理数据
|
||
func processNMEAData(data []byte, lcx6xz *LCX6XZ) []byte {
|
||
processed := 0
|
||
|
||
for i := 0; i < len(data); {
|
||
// 查找NMEA语句开始标记 '$'
|
||
if data[i] == 0x24 {
|
||
// 查找完整的NMEA语句（以\r\n结尾）
|
||
endPos := findNMEAEnd(data[i:])
|
||
if endPos == -1 {
|
||
// 没有找到完整语句，保留剩余数据
|
||
break
|
||
}
|
||
|
||
// 解析NMEA语句
|
||
sentence := data[i : i+endPos]
|
||
err := parseNMEASentence(sentence, lcx6xz)
|
||
if err != nil {
|
||
fmt.Printf("NMEA解析错误: %v\n", err)
|
||
}
|
||
|
||
// 移动到下一个位置
|
||
i += endPos
|
||
processed = i
|
||
|
||
} else if i < len(data)-1 && data[i] == 0xF1 && data[i+1] == 0xD9 {
|
||
// 处理二进制协议（如果需要）
|
||
skip, err := ParsBM(data[i:], lcx6xz)
|
||
if err != nil {
|
||
fmt.Printf("二进制协议解析错误: %v\n", err)
|
||
i++ // 跳过当前字节
|
||
} else {
|
||
i += skip
|
||
processed = i
|
||
}
|
||
} else {
|
||
// 跳过无效字节
|
||
i++
|
||
}
|
||
}
|
||
|
||
// 返回未处理的数据
|
||
if processed > 0 {
|
||
return data[processed:]
|
||
}
|
||
return data
|
||
}
|
||
|
||
// findNMEAEnd 查找NMEA语句的结束位置
|
||
func findNMEAEnd(data []byte) int {
|
||
for i := 0; i < len(data)-1; i++ {
|
||
if data[i] == '\r' && data[i+1] == '\n' {
|
||
return i + 2 // 包含\r\n
|
||
}
|
||
}
|
||
return -1 // 没有找到结束标记
|
||
}
|
||
|
||
// parseNMEASentence 解析单个NMEA语句
|
||
func parseNMEASentence(sentence []byte, lcx6xz *LCX6XZ) error {
|
||
// 移除\r\n
|
||
sentenceStr := string(sentence)
|
||
if len(sentenceStr) >= 2 && sentenceStr[len(sentenceStr)-2:] == "\r\n" {
|
||
sentenceStr = sentenceStr[:len(sentenceStr)-2]
|
||
}
|
||
|
||
// 验证最小长度
|
||
if len(sentenceStr) < 6 {
|
||
return fmt.Errorf("NMEA语句太短: %s", sentenceStr)
|
||
}
|
||
|
||
// 解析NMEA语句类型
|
||
nmeaType := ParsNMEAType(sentenceStr, len(sentenceStr))
|
||
|
||
// 加锁保护共享数据
|
||
lcx6xz.mutex.Lock()
|
||
defer lcx6xz.mutex.Unlock()
|
||
|
||
switch nmeaType {
|
||
case NMEA_RMC_TYPE:
|
||
rmc := ParsNMEARMC(sentenceStr, len(sentenceStr))
|
||
if rmc != nil {
|
||
lcx6xz.NMEA_RMC = rmc
|
||
fmt.Printf("✅ RMC: 时间=%s, 纬度=%s%s, 经度=%s%s, 状态=%s\n",
|
||
trimNullBytes(rmc.UTC[:]), trimNullBytes(rmc.Lat[:]), trimNullBytes(rmc.N_S[:]),
|
||
trimNullBytes(rmc.Lon[:]), trimNullBytes(rmc.E_W[:]), trimNullBytes(rmc.Status[:]))
|
||
}
|
||
case NMEA_GGA_TYPE:
|
||
gga := ParsNMEAGGA(sentenceStr, len(sentenceStr))
|
||
if gga != nil {
|
||
lcx6xz.NMEA_GGA = gga // 存储GGA数据
|
||
fmt.Printf("✅ GGA: 时间=%s, 纬度=%s%s, 经度=%s%s, 质量=%s, 卫星数=%s\n",
|
||
trimNullBytes(gga.UTC[:]), trimNullBytes(gga.Lat[:]), trimNullBytes(gga.N_S[:]),
|
||
trimNullBytes(gga.Lon[:]), trimNullBytes(gga.E_W[:]),
|
||
trimNullBytes(gga.Quality[:]), trimNullBytes(gga.NumSatUsed[:]))
|
||
}
|
||
case NMEA_GLL_TYPE:
|
||
gll := ParsNMEAGLL(sentenceStr, len(sentenceStr))
|
||
if gll != nil {
|
||
lcx6xz.NMEA_GLL = gll // 存储GLL数据
|
||
fmt.Printf("✅ GLL: 纬度=%s%s, 经度=%s%s, 时间=%s, 状态=%s\n",
|
||
trimNullBytes(gll.Lat[:]), trimNullBytes(gll.N_S[:]),
|
||
trimNullBytes(gll.Lon[:]), trimNullBytes(gll.E_W[:]),
|
||
trimNullBytes(gll.UTC[:]), trimNullBytes(gll.Status[:]))
|
||
}
|
||
case NMEA_GSA_TYPE:
|
||
gsa := ParsNMEAGSA(sentenceStr, len(sentenceStr))
|
||
if gsa != nil {
|
||
lcx6xz.NMEA_GSA = gsa // 存储GSA数据
|
||
fmt.Printf("✅ GSA: 模式=%s, 定位模式=%s, PDOP=%s, HDOP=%s, VDOP=%s\n",
|
||
trimNullBytes(gsa.Mode[:]), trimNullBytes(gsa.FixMode[:]),
|
||
trimNullBytes(gsa.PDOP[:]), trimNullBytes(gsa.HDOP[:]), trimNullBytes(gsa.VDOP[:]))
|
||
}
|
||
case NMEA_GSV_TYPE:
|
||
gsv := ParsNMEAGSV(sentenceStr, len(sentenceStr))
|
||
if gsv != nil {
|
||
lcx6xz.NMEA_GSV = gsv // 存储GSV数据
|
||
fmt.Printf("✅ GSV: 总语句数=%s, 语句号=%s, 可视卫星数=%s\n",
|
||
trimNullBytes(gsv.TotalNumSen[:]), trimNullBytes(gsv.SenNum[:]),
|
||
trimNullBytes(gsv.TotalNumSat[:]))
|
||
}
|
||
case NMEA_VTG_TYPE:
|
||
vtg := ParsNMEAVTG(sentenceStr, len(sentenceStr))
|
||
if vtg != nil {
|
||
lcx6xz.NMEA_VTG = vtg // 存储VTG数据
|
||
fmt.Printf("✅ VTG: 航向=%s, 速度(节)=%s, 速度(km/h)=%s\n",
|
||
trimNullBytes(vtg.COGT[:]), trimNullBytes(vtg.SOGN[:]), trimNullBytes(vtg.SOGK[:]))
|
||
}
|
||
default:
|
||
if len(sentenceStr) >= 6 {
|
||
fmt.Printf("⚠️ 未知NMEA语句类型: %s\n", sentenceStr[:6])
|
||
}
|
||
}
|
||
|
||
return nil
|
||
}
|
||
|
||
// trimNullBytes 移除字节数组中的空字节
|
||
func trimNullBytes(data []byte) string {
|
||
for i, b := range data {
|
||
if b == 0 {
|
||
return string(data[:i])
|
||
}
|
||
}
|
||
return string(data)
|
||
}
|
||
|
||
// ParsNMEA 解析NMEA协议语句（保持向后兼容）
|
||
func ParsNMEA(buffer []byte, lcx6xz *LCX6XZ) (int, error) {
|
||
// 使用新的解析函数
|
||
err := parseNMEASentence(buffer, lcx6xz)
|
||
if err != nil {
|
||
return 0, err
|
||
}
|
||
|
||
// 查找结束位置
|
||
endPos := findNMEAEnd(buffer)
|
||
if endPos == -1 {
|
||
return 0, errors.New("incomplete NMEA sentence")
|
||
}
|
||
|
||
return endPos, nil
|
||
}
|
||
|
||
// ParsBM 解析二进制协议语句
|
||
func ParsBM(buffer []byte, lcx6xz *LCX6XZ) (int, error) {
|
||
if len(buffer) < 8 {
|
||
return 0, errors.New("二进制消息太短")
|
||
}
|
||
|
||
// 检查帧头 0xF1 0xD9
|
||
if buffer[0] != 0xF1 || buffer[1] != 0xD9 {
|
||
return 0, errors.New("无效的二进制消息头")
|
||
}
|
||
|
||
groupID := buffer[2]
|
||
subID := buffer[3]
|
||
length := uint16(buffer[4]) | (uint16(buffer[5]) << 8)
|
||
|
||
// 检查消息长度
|
||
totalLen := int(6 + length + 2) // 头部6字节 + 载荷 + 校验和2字节
|
||
if len(buffer) < totalLen {
|
||
return 0, errors.New("二进制消息数据不完整")
|
||
}
|
||
|
||
// 验证校验和
|
||
checksum := QlCheckQuectel(buffer[:totalLen-2])
|
||
receivedChecksum := uint16(buffer[totalLen-2]) | (uint16(buffer[totalLen-1]) << 8)
|
||
|
||
if checksum != receivedChecksum {
|
||
return 0, fmt.Errorf("校验和错误: 期望 %04X, 实际 %04X", checksum, receivedChecksum)
|
||
}
|
||
|
||
// 处理不同类型的二进制消息
|
||
switch groupID {
|
||
case 0x05: // BIN_RES_GID - 响应消息
|
||
switch subID {
|
||
case 0x01: // ACK
|
||
fmt.Printf("✅ 收到ACK确认: GroupID=0x%02X, SubID=0x%02X\n",
|
||
buffer[6], buffer[7])
|
||
case 0x00: // NAK
|
||
fmt.Printf("❌ 收到NAK否认: GroupID=0x%02X, SubID=0x%02X\n",
|
||
buffer[6], buffer[7])
|
||
}
|
||
case 0x06: // BIN_CFG_GID - 配置消息响应
|
||
switch subID {
|
||
case 0x01: // MSG配置响应
|
||
if length >= 3 {
|
||
targetGroupID := buffer[6]
|
||
targetSubID := buffer[7]
|
||
outputRate := buffer[8]
|
||
fmt.Printf("📊 输出速率响应: NMEA类型=0x%02X%02X, 速率=%d\n",
|
||
targetGroupID, targetSubID, outputRate)
|
||
|
||
// 存储查询结果到设备结构中
|
||
if lcx6xz != nil {
|
||
lcx6xz.mutex.Lock()
|
||
if lcx6xz.OutputRates == nil {
|
||
lcx6xz.OutputRates = make(map[NMEA_SUB_ID]uint8)
|
||
}
|
||
lcx6xz.OutputRates[NMEA_SUB_ID(targetSubID)] = outputRate
|
||
lcx6xz.mutex.Unlock()
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
return totalLen, nil
|
||
}
|
||
|
||
// SendBinaryCommand 发送二进制命令到GPS设备
|
||
func SendBinaryCommand(lcx6xz *LCX6XZ, data []byte) error {
|
||
if lcx6xz == nil || lcx6xz.uartFd == nil {
|
||
return errors.New("GPS设备未连接")
|
||
}
|
||
|
||
fmt.Printf("📤 发送二进制命令: %X\n", data)
|
||
|
||
_, err := lcx6xz.uartFd.Write(data)
|
||
if err != nil {
|
||
return fmt.Errorf("发送二进制命令失败: %v", err)
|
||
}
|
||
|
||
return nil
|
||
}
|
||
|
||
// SetNMEAOutputRate 设置NMEA消息输出速率
|
||
func SetNMEAOutputRate(lcx6xz *LCX6XZ, nmeaType NMEA_SUB_ID, rate uint8) error {
|
||
// 创建设置输出速率的配置消息
|
||
cfgMsg := CfgMsgSetOutRate(NMEA_GID, nmeaType, rate)
|
||
if cfgMsg == nil {
|
||
return errors.New("创建配置消息失败")
|
||
}
|
||
|
||
// 发送配置消息
|
||
msgBytes := cfgMsg.ToBytes()
|
||
if msgBytes == nil {
|
||
return errors.New("转换配置消息失败")
|
||
}
|
||
|
||
return SendBinaryCommand(lcx6xz, msgBytes)
|
||
}
|
||
|
||
// GetNMEAOutputRate 查询NMEA消息输出速率
|
||
func GetNMEAOutputRate(lcx6xz *LCX6XZ, nmeaType NMEA_SUB_ID) error {
|
||
// 创建查询输出速率的配置消息
|
||
cfgMsg := CfgMsgQueOutRate(NMEA_GID, nmeaType)
|
||
if cfgMsg == nil {
|
||
return errors.New("创建查询消息失败")
|
||
}
|
||
|
||
// 发送查询消息
|
||
msgBytes := cfgMsg.ToBytes()
|
||
if msgBytes == nil {
|
||
return errors.New("转换查询消息失败")
|
||
}
|
||
|
||
return SendBinaryCommand(lcx6xz, msgBytes)
|
||
}
|
||
|
||
// 初始化LCX6XZ
|
||
func InitLCX6XZ(Name string, Baud int, ReadTimeout int) (*LCX6XZ, error) {
|
||
lcx6xz := &LCX6XZ{
|
||
OutputRates: make(map[NMEA_SUB_ID]uint8),
|
||
ResData: make([]byte, 1024),
|
||
uartAckCh: make(chan struct{}),
|
||
}
|
||
|
||
// 配置串口
|
||
config := &serial.Config{
|
||
Name: Name,
|
||
Baud: Baud,
|
||
Parity: serial.ParityNone,
|
||
StopBits: serial.Stop1,
|
||
ReadTimeout: time.Duration(ReadTimeout) * time.Millisecond,
|
||
}
|
||
|
||
port, err := serial.OpenPort(config)
|
||
if err != nil {
|
||
return nil, fmt.Errorf("open uart device: %w", err)
|
||
}
|
||
|
||
lcx6xz.uartFd = port
|
||
|
||
// 启动接收任务
|
||
go UartRX_Task(lcx6xz)
|
||
|
||
return lcx6xz, nil
|
||
}
|