From 3711fa98ce2bdfff7e6e089af462c71138ac3242 Mon Sep 17 00:00:00 2001
From: xzb <223@qq.com>
Date: Mon, 20 Nov 2023 16:55:07 +0800
Subject: [PATCH] balise btm

---
 third_party/message/can_btm_data.go |  13 +++
 third_party/message/can_btm_rsp.go  | 120 ++++++++++++++++++++++++++++
 third_party/message/can_bus.go      |  13 +++
 3 files changed, 146 insertions(+)
 create mode 100644 third_party/message/can_btm_rsp.go

diff --git a/third_party/message/can_btm_data.go b/third_party/message/can_btm_data.go
index c14d9e1..5718566 100644
--- a/third_party/message/can_btm_data.go
+++ b/third_party/message/can_btm_data.go
@@ -80,6 +80,7 @@ func (f *BtmDataMessageTimeAFrame) Encode() CanBusData {
 	//
 	return writer.(CanBusData)
 }
+
 func (f *BtmDataMessageTimeAFrame) Decode(reader CanBitsReader) bool {
 	if !f.FId.Decode(reader) {
 		return false
@@ -328,3 +329,15 @@ func (f *BtmDataMessageEndFrame) Decode(reader CanBitsReader) bool {
 	//
 	return true
 }
+
+// 32位时间戳，转换成字节数组
+// 大端模型
+func canTimeToBytes(ts uint32) []byte {
+	rt := make([]byte, 0, 4)
+	byteMsk := uint32(0x00_00_00_ff)
+	rt = append(rt, byte((ts>>24)&byteMsk))
+	rt = append(rt, byte((ts>>16)&byteMsk))
+	rt = append(rt, byte((ts>>8)&byteMsk))
+	rt = append(rt, byte(ts&byteMsk))
+	return rt
+}
diff --git a/third_party/message/can_btm_rsp.go b/third_party/message/can_btm_rsp.go
new file mode 100644
index 0000000..dadc599
--- /dev/null
+++ b/third_party/message/can_btm_rsp.go
@@ -0,0 +1,120 @@
+package message
+
+import "log/slog"
+
+// CreateBtmRspFramesData 数据帧与状态应答帧同时发送给ATP
+// 共17帧，17X12个字节，每个帧12字节
+// msg - 应答器报文
+// msgPackError - true BTM解包发生错误，则数据帧及CRC32A/B全填“0xFF”
+func CreateBtmRspFramesData(statusRsp *BtmStatusRspFrame, msg []byte, msgPackError bool, msgTimeA uint32, msgTimeB uint32, tkTimeB uint32) ([]byte, bool) {
+	if len(msg) > 104 { //数据帧最多存储13*8个字节
+		return nil, false
+	}
+	//最近一次ATP查询请求序列号
+	sn := statusRsp.FId.ID4
+	//13个BtmDataMessageFrame [0x00,0x0c]
+	dms := make([]*BtmDataMessageFrame, 13)
+	for mr := 0x00; mr <= 0x0c; mr++ {
+		dms[mr] = NewBtmDataMessageFrame(sn, byte(mr))
+		dms[mr].Message = make([]byte, 8) //8字节数组，默认值0
+		//
+		if !msgPackError {
+			mi := mr * 8
+			if mi < len(msg) { //数据帧中有<=8个字节数据
+				if mi+7 < len(msg) {
+					dms[mr].Message = msg[mi : mi+8]
+				} else {
+					for i, d := range msg[mi:] {
+						dms[mr].Message[i] = d
+					}
+				}
+			}
+		} else { //BTM解包发生错误，则数据帧及CRC32A/B全填“0xFF”
+			for c := 0; c < 8; c++ {
+				dms[mr].Message[c] = 0xff
+			}
+		}
+	}
+	//
+	dtA := NewBtmDataMessageTimeAFrame(sn)
+	dtA.TimeA = msgTimeA
+	if !msgPackError {
+		var crc32AData []byte
+		crc32AData = append(crc32AData, msg...)
+		crc32AData = append(crc32AData, canTimeToBytes(dtA.TimeA)...)
+		dtA.Crc32A = calculateDataRspCrc32A(crc32AData) //CRC32A的校验范围是：报文+时间戳A
+	} else { //BTM解包发生错误，则数据帧及CRC32A/B全填“0xFF”
+		dtA.Crc32A = 0xff_ff_ff_ff
+	}
+	//
+	dtB := NewBtmDataMessageTimeBFrame(sn)
+	dtB.TimeB = msgTimeB
+	if !msgPackError {
+		var crc32BData []byte
+		crc32BData = append(crc32BData, msg...)
+		crc32BData = append(crc32BData, canTimeToBytes(dtB.TimeB)...)
+		dtB.Crc32B = calculateDataRspCrc32B(crc32BData) //CRC32B的校验范围是：报文+时间戳B
+	} else { //BTM解包发生错误，则数据帧及CRC32A/B全填“0xFF”
+		dtB.Crc32B = 0xff_ff_ff_ff
+	}
+	//
+	end := NewBtmDataMessageEndFrame(sn)
+	end.TkB = tkTimeB
+	//
+	crc32CData := make([]byte, 0, 132) //17*8-4
+	rt := make([]byte, 0, 204)         //17*12
+	// 状态应答帧
+	statusData := statusRsp.Encode()
+	crc32CData = append(crc32CData, canFrameContent(statusData)...)
+	rt = append(rt, statusData.GetData()...)
+	// 数据帧
+	for _, dm := range dms {
+		dmData := dm.Encode()
+		crc32CData = append(crc32CData, canFrameContent(dmData)...)
+		rt = append(rt, dmData.GetData()...)
+	}
+	// 消息时刻A
+	dtAData := dtA.Encode()
+	crc32CData = append(crc32CData, canFrameContent(dtAData)...)
+	rt = append(rt, dtAData.GetData()...)
+	// 消息时刻B
+	dtBData := dtB.Encode()
+	crc32CData = append(crc32CData, canFrameContent(dtBData)...)
+	rt = append(rt, dtBData.GetData()...)
+	// 消息结束帧
+	crc32CData = append(crc32CData, canTimeToBytes(end.TkB)...)
+	if len(crc32CData) != 132 {
+		slog.Warn("BtmDataMessageEndFrame crc32 校验数据须132字节")
+		return nil, false
+	}
+	end.Crc32C = calculateDataRspCrc32C(crc32CData)
+	rt = append(rt, end.Encode().GetData()...)
+	if len(rt) != 204 {
+		slog.Warn("BTM响应ATP 17帧须136字节")
+		return nil, false
+	}
+	//
+	return rt, true
+}
+
+// CAN帧中除去开始29bits的剩余8字节数据
+func canFrameContent(canFrame CanBusData) []byte {
+	rd := NewCanBitsReader(canFrame.GetData(), canFrame.GetLastRowBitsLen())
+	rd.ReadByte()  //ID1
+	rd.ReadByte()  //ID2
+	rd.ReadByte()  //ID3
+	rd.ReadBits(5) //ID4
+	//
+	rt := make([]byte, 0, 8)
+	for {
+		d, ok := rd.ReadByte()
+		if !ok {
+			break
+		}
+		rt = append(rt, d)
+	}
+	if len(rt) != 8 {
+		panic("CAN帧中数据体须为8字节")
+	}
+	return rt
+}
diff --git a/third_party/message/can_bus.go b/third_party/message/can_bus.go
index 7720a61..a59d659 100644
--- a/third_party/message/can_bus.go
+++ b/third_party/message/can_bus.go
@@ -218,3 +218,16 @@ func calculateAtpReqCrc16(data []byte) uint16 {
 	crc := CrcTableBased(data, 16, 0, false, false, 0, crc16AtpReqTable)
 	return uint16(crc)
 }
+
+// CRC32A的校验范围是：报文+时间戳A
+func calculateDataRspCrc32A(data []byte) uint32 {
+	return 0
+}
+
+// CRC32B的校验范围是：报文+时间戳B
+func calculateDataRspCrc32B(data []byte) uint32 {
+	return 0
+}
+func calculateDataRspCrc32C(data []byte) uint32 {
+	return 0
+}