From 0653b3a7ff49ff1a7c9a21ef4bcf9fc2802d2313 Mon Sep 17 00:00:00 2001
From: xzb <223@qq.com>
Date: Thu, 16 Nov 2023 17:21:16 +0800
Subject: [PATCH] =?UTF-8?q?=E4=BF=A1=E5=8F=B7=E6=9C=BA=E6=A8=A1=E5=9E=8B?=
 =?UTF-8?q?=E6=9E=9A=E4=B8=BE?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 third_party/message/balise_btm.go | 195 ++++++++++++++++++++++++++++++
 1 file changed, 195 insertions(+)
 create mode 100644 third_party/message/balise_btm.go

diff --git a/third_party/message/balise_btm.go b/third_party/message/balise_btm.go
new file mode 100644
index 0000000..9038948
--- /dev/null
+++ b/third_party/message/balise_btm.go
@@ -0,0 +1,195 @@
+package message
+
+//应答器传输模块BTM与车载ATP通信协议定义
+
+const (
+	CAN_ADDR_REQ_BTM        = byte(0x62) //设备地址
+	CAN_ADDR_REQ_ATP        = byte(0x81) //设备地址
+	CAN_ADDR_RSP_BTM        = byte(0x02) //设备地址
+	CAN_ADDR_RSP_ATP        = byte(0x01) //设备地址
+	CAN_FRAME_ATP_REQ       = byte(0x02) //帧类型-查询请求帧
+	CAN_FRAME_STATUS_RSP    = byte(0x01) //状态应答帧
+	CAN_FRAME_TIME_SYNC_RSP = byte(0x02) //时间同步应答帧
+	CAN_FRAME_DATA_RSP      = byte(0x80) //数据应答帧
+	CAN_FRAME_DATA_END_RSP  = byte(0xff) //数据结束应答帧
+)
+
+// CanCodec CAN总线数据编解码定义
+type CanCodec interface {
+	Encode() CanBits
+	Decode(bits CanBits)
+}
+
+////////////////////////////////////////////////////////////////////////////////////
+
+// CanFrameId CAN串行总线帧ID定义,29bits
+type CanFrameId struct {
+	//ID1 8bits
+	ID1 byte
+	//ID2 8bits
+	ID2 byte
+	//ID3 8bits
+	ID3 byte
+	//ID4 5bits ,有效位[bit4,bit0]，无效位[bit7,bit5]
+	ID4 byte
+}
+
+func (f *CanFrameId) Encode(bits *CanBits) {
+	bits.AddByte(f.ID1)
+	bits.AddByte(f.ID2)
+	bits.AddByte(f.ID3)
+	bits.AddBits(f.ID4, 5)
+}
+
+// NewCanFrameId 创建CAN串行总线帧ID
+// dstAddr 目的设备地址
+// srcAddr 源设备地址
+// frameType 帧类型
+// sn 序列号
+func NewCanFrameId(dstAddr byte, srcAddr byte, frameType byte, sn byte) *CanFrameId {
+	return &CanFrameId{
+		ID1: dstAddr,
+		ID2: srcAddr,
+		ID3: frameType,
+		ID4: sn,
+	}
+}
+
+////////////////////////////////////////////////////////////////
+
+// AtpRequestFrame ATP查询帧
+type AtpRequestFrame struct {
+	//帧ID
+	FId CanFrameId
+	//功率放大器命令，false-0关闭功率放大器，true-1s开启功率放大器
+	PowerAmplifierTurnOn bool
+	//功率放大器控制者，false-0BTM控制，true-1ATP控制
+	PowerAmplifierControlledByAtp bool
+	//重发请求
+	ResendRequest byte
+	//速度,1=0.1km/h
+	Speed uint16
+	//当前ATP系统时间，1=1ms
+	time uint32
+	//crc 校验
+	Crc16 uint16
+}
+
+// NewAtpRequestFrame 创建ATP查询帧
+func NewAtpRequestFrame(sn byte) *AtpRequestFrame {
+	return &AtpRequestFrame{
+		FId: *NewCanFrameId(CAN_ADDR_REQ_BTM, CAN_ADDR_REQ_ATP, CAN_FRAME_ATP_REQ, sn),
+	}
+}
+func (f *AtpRequestFrame) Encode() *CanBits {
+	bits := NewCanBits(12)
+	//
+	f.FId.Encode(bits)
+	//
+	if f.PowerAmplifierTurnOn {
+		bits.AddBits(1, 1)
+	} else {
+		bits.AddBits(0, 1)
+	}
+	//
+	if f.PowerAmplifierControlledByAtp {
+		bits.AddBits(1, 1)
+	} else {
+		bits.AddBits(0, 1)
+	}
+	//
+	bits.AddBits(f.ResendRequest, 2)
+	//速度
+	bits.AddBits(byte(f.Speed>>8), 4)
+	bits.AddByte(byte(f.Speed))
+	//时间
+
+	//
+	return bits
+}
+
+/////////////////////////////////////////////
+
+// CanBits 可以在CAN总线上传输的bit流
+// 按bit位来存储数据
+// 一个字节中bit位编号，从高位到低位依次为bit7-bit0
+type CanBits struct {
+	Data        []byte
+	LastBitsLen int //Data中最后一个字节中从高位到低位存储数据的有效位数，值范围[1,8],8-LastBitsLen则为最后一个字节中低位剩余的空位数量
+}
+
+func NewCanBits(cap int) *CanBits {
+	return &CanBits{LastBitsLen: 8, Data: make([]byte, 0, cap)}
+}
+
+// AddByte 添加一整个字节bit数据
+func (b *CanBits) AddByte(bits byte) {
+	b.AddBits(bits, 8)
+}
+
+const (
+	CAN_BITS_LEN1 = byte(0b0000_0001)
+	CAN_BITS_LEN2 = byte(0b0000_0011)
+	CAN_BITS_LEN3 = byte(0b0000_0111)
+	CAN_BITS_LEN4 = byte(0b0000_1111)
+	CAN_BITS_LEN5 = byte(0b0001_1111)
+	CAN_BITS_LEN6 = byte(0b0011_1111)
+	CAN_BITS_LEN7 = byte(0b0111_1111)
+	CAN_BITS_LEN8 = byte(0b1111_1111)
+)
+
+func getCanBitsMask(bitsLen int) byte {
+	switch bitsLen {
+	case 1:
+		return CAN_BITS_LEN1
+	case 2:
+		return CAN_BITS_LEN2
+	case 3:
+		return CAN_BITS_LEN3
+	case 4:
+		return CAN_BITS_LEN4
+	case 5:
+		return CAN_BITS_LEN5
+	case 6:
+		return CAN_BITS_LEN6
+	case 7:
+		return CAN_BITS_LEN7
+	case 8:
+		return CAN_BITS_LEN8
+	default:
+		panic("bitsLen<1||bitsLen>8")
+	}
+}
+
+// AddBits 添加bit数据，bitsLen-bits字节中有效位数，有效位向低位对齐
+func (b *CanBits) AddBits(bits byte, bitsLen int) {
+	if bitsLen < 1 || bitsLen > 8 {
+		panic("bitsLen<1||bitsLen>8")
+	}
+	if len(b.Data) <= 0 {
+		b.LastBitsLen = 8
+	}
+	//
+	bits &= getCanBitsMask(bitsLen)
+	//
+	lastByteHavePos := b.LastBitsLen < 8
+	if lastByteHavePos {
+		lastBytePosWidth := 8 - b.LastBitsLen //最后一个字节低位剩余可用bit位
+		if bitsLen <= lastBytePosWidth {      //最后一个字节有足够的空位
+			bits <<= lastBytePosWidth - bitsLen
+			b.Data[len(b.Data)-1] |= bits
+			b.LastBitsLen += bitsLen
+		} else { //最后一个字节没有足够的空位
+			lb := bits >> (bitsLen - lastBytePosWidth)
+			b.Data[len(b.Data)-1] |= lb //至此最后一个字节中的位存满
+			//
+			nb := bits << (8 - (bitsLen - lastBytePosWidth))
+			b.Data = append(b.Data, nb)
+			b.LastBitsLen = bitsLen - lastBytePosWidth
+		}
+	} else {
+		bits <<= 8 - bitsLen
+		b.Data = append(b.Data, bits)
+		b.LastBitsLen = bitsLen
+	}
+}