balise btm
This commit is contained in:
xzb
parent
2355c8c518
commit
21b5904646
229
third_party/message/balise_btm.go
vendored
229
third_party/message/balise_btm.go
vendored
@ -1,229 +0,0 @@
|
||||
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) Decode(bits *CanBits) {
|
||||
|
||||
}
|
||||
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))
|
||||
//时间
|
||||
timeArr := make([]byte, 4, 4)
|
||||
timeArr[0] = byte(f.time >> 24)
|
||||
timeArr[1] = byte(f.time >> 16)
|
||||
timeArr[2] = byte(f.time >> 8)
|
||||
timeArr[3] = byte(f.time)
|
||||
for _, timeByte := range timeArr {
|
||||
bits.AddByte(timeByte)
|
||||
}
|
||||
//crc
|
||||
crc16 := calculateAtpReqCrc16(timeArr)
|
||||
bits.AddByte(byte(crc16 >> 8))
|
||||
bits.AddByte(byte(crc16))
|
||||
//
|
||||
return bits
|
||||
}
|
||||
|
||||
///////////////////// CRC ////////////////////////
|
||||
|
||||
const (
|
||||
CAN_CRC16_ATPREQ = 0x11021
|
||||
)
|
||||
|
||||
var (
|
||||
crc16AtpReqTable []uint32 = nil
|
||||
)
|
||||
|
||||
func CanCreateCrcTable() {
|
||||
if crc16AtpReqTable == nil {
|
||||
crc16AtpReqTable = CreateCrcTable(CAN_CRC16_ATPREQ, 16, false)
|
||||
}
|
||||
}
|
||||
func calculateAtpReqCrc16(data []byte) uint16 {
|
||||
crc := CrcTableBased(data, 16, 0, false, false, 0, crc16AtpReqTable)
|
||||
return uint16(crc)
|
||||
}
|
||||
|
||||
///////////////////// bit 流处理 ////////////////////////
|
||||
|
||||
// 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
|
||||
}
|
||||
}
|
||||
225
third_party/message/btm_atp_req.go
vendored
Normal file
225
third_party/message/btm_atp_req.go
vendored
Normal file
@ -0,0 +1,225 @@
|
||||
package message
|
||||
|
||||
import "log/slog"
|
||||
|
||||
//应答器传输模块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) //数据结束应答帧
|
||||
)
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// 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 CanBitsWriter) {
|
||||
bits.AddByte(f.ID1)
|
||||
bits.AddByte(f.ID2)
|
||||
bits.AddByte(f.ID3)
|
||||
bits.AddBits(f.ID4, 5)
|
||||
}
|
||||
func (f *CanFrameId) Decode(bits CanBitsReader) bool {
|
||||
if id, ok := bits.ReadByte(); ok {
|
||||
f.ID1 = id
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if id, ok := bits.ReadByte(); ok {
|
||||
f.ID2 = id
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if id, ok := bits.ReadByte(); ok {
|
||||
f.ID3 = id
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if id, ok := bits.ReadBits(5); ok {
|
||||
f.ID4 = id
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
return true
|
||||
}
|
||||
|
||||
// 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) Decode(bits CanBitsReader) bool {
|
||||
if !f.FId.Decode(bits) {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadBits(1); ok {
|
||||
f.PowerAmplifierTurnOn = d == 1
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadBits(1); ok {
|
||||
f.PowerAmplifierControlledByAtp = d == 1
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadBits(2); ok {
|
||||
f.ResendRequest = d
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
//speed
|
||||
f.Speed = 0x0000
|
||||
if d, ok := bits.ReadBits(4); ok {
|
||||
f.Speed = uint16(d)
|
||||
f.Speed <<= 8
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
f.Speed |= uint16(d)
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
//time
|
||||
f.Time = 0
|
||||
timeArr := make([]byte, 4, 4)
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
timeArr[0] = d
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
timeArr[1] = d
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
timeArr[2] = d
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
timeArr[3] = d
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
crc16 := calculateAtpReqCrc16(timeArr)
|
||||
t1 := uint32(timeArr[0])
|
||||
t2 := uint32(timeArr[1])
|
||||
t3 := uint32(timeArr[2])
|
||||
t4 := uint32(timeArr[3])
|
||||
f.Time |= t1 << 24
|
||||
f.Time |= t2 << 16
|
||||
f.Time |= t3 << 8
|
||||
f.Time |= t4
|
||||
//crc
|
||||
f.Crc16 = 0
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
crc1 := uint16(d)
|
||||
f.Crc16 |= crc1 << 8
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
if d, ok := bits.ReadByte(); ok {
|
||||
crc2 := uint16(d)
|
||||
f.Crc16 |= crc2
|
||||
} else {
|
||||
return false
|
||||
}
|
||||
//crc校验
|
||||
if f.Crc16 != crc16 {
|
||||
slog.Debug("解码AtpRequestFrame，CRC16校验未通过！！")
|
||||
return false
|
||||
}
|
||||
//
|
||||
return true
|
||||
}
|
||||
func (f *AtpRequestFrame) Encode() CanBusData {
|
||||
bits := NewCanBitsWriter(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))
|
||||
//时间
|
||||
timeArr := make([]byte, 4, 4)
|
||||
timeArr[0] = byte(f.Time >> 24)
|
||||
timeArr[1] = byte(f.Time >> 16)
|
||||
timeArr[2] = byte(f.Time >> 8)
|
||||
timeArr[3] = byte(f.Time)
|
||||
for _, timeByte := range timeArr {
|
||||
bits.AddByte(timeByte)
|
||||
}
|
||||
//crc
|
||||
crc16 := calculateAtpReqCrc16(timeArr)
|
||||
bits.AddByte(byte(crc16 >> 8))
|
||||
bits.AddByte(byte(crc16))
|
||||
//
|
||||
return bits.(CanBusData)
|
||||
}
|
||||
219
third_party/message/btm_bus_can.go
vendored
Normal file
219
third_party/message/btm_bus_can.go
vendored
Normal file
@ -0,0 +1,219 @@
|
||||
package message
|
||||
|
||||
/////////////////////CAN串行总线 bit 流处理 ////////////////////////
|
||||
|
||||
// CanBits 可以在CAN总线上传输的bit流
|
||||
// 按bit位来存储数据
|
||||
// 一个字节中bit位编号，从高位到低位依次为bit7-bit0
|
||||
type CanBits struct {
|
||||
Data []byte
|
||||
LastBitsLen int //Data中最后一个字节中从高位到低位存储数据的有效位数，值范围[1,8],8-LastBitsLen则为最后一个字节中低位剩余的空位数量
|
||||
readRowIndex int //行读指针，Data中的一个字节为一行，从0开始；当前值表示当前可读位置
|
||||
readColIndex int //列读指针,字节中的bit位为列，从7开始到0，字节中最左侧bit为最高位编号7；当前值表示当前可读位置;当值为-1时表示已经读结束
|
||||
}
|
||||
|
||||
// CanBusData CAN总线上传输的数据
|
||||
type CanBusData interface {
|
||||
GetData() []byte
|
||||
GetLastRowBitsLen() int
|
||||
}
|
||||
type CanBitsReader interface {
|
||||
// ReadByte 读取一个字节
|
||||
ReadByte() (byte, bool)
|
||||
// ReadBits 读取bits,一次最多8bits
|
||||
ReadBits(bitsLen int) (byte, bool)
|
||||
}
|
||||
type CanBitsWriter interface {
|
||||
// AddByte 添加一整个字节bit数据
|
||||
AddByte(bits byte) bool
|
||||
// AddBits 添加bit数据，bitsLen-bits字节中有效位数，有效位向低位对齐
|
||||
AddBits(bits byte, bitsLen int) bool
|
||||
//GetReader 获取CanBits reader
|
||||
GetReader() CanBitsReader
|
||||
}
|
||||
|
||||
func NewCanBitsWriter(cap int) CanBitsWriter {
|
||||
return &CanBits{LastBitsLen: 8, Data: make([]byte, 0, cap)}
|
||||
}
|
||||
func NewCanBitsReader(data []byte, lastRowBitsLen int) CanBitsReader {
|
||||
return &CanBits{
|
||||
LastBitsLen: lastRowBitsLen,
|
||||
Data: data,
|
||||
readRowIndex: 0,
|
||||
readColIndex: 7,
|
||||
}
|
||||
}
|
||||
func (b *CanBits) GetData() []byte {
|
||||
return b.Data
|
||||
}
|
||||
func (b *CanBits) GetLastRowBitsLen() int {
|
||||
return b.LastBitsLen
|
||||
}
|
||||
func (b *CanBits) GetReader() CanBitsReader {
|
||||
if len(b.Data) > 0 {
|
||||
b.readRowIndex = 0
|
||||
b.readColIndex = 7
|
||||
} else {
|
||||
b.readRowIndex = -1
|
||||
}
|
||||
return b
|
||||
}
|
||||
|
||||
// ReadByte 读取一个字节
|
||||
func (b *CanBits) ReadByte() (byte, bool) {
|
||||
return b.ReadBits(8)
|
||||
}
|
||||
|
||||
// ReadBits 读取bits,一次最多8bits
|
||||
func (b *CanBits) ReadBits(bitsLen int) (byte, bool) {
|
||||
if bitsLen < 1 || bitsLen > 8 {
|
||||
return 0, false
|
||||
}
|
||||
//当前可读行可读bits数
|
||||
if !b.isReadEnd() {
|
||||
rowBitsLen := b.curRowCanReadLen()
|
||||
if bitsLen <= rowBitsLen { //当前行有足够的bits
|
||||
sb := b.Data[b.readRowIndex]
|
||||
sb >>= b.readColIndex - bitsLen + 1
|
||||
b.toNextReadPos(bitsLen)
|
||||
return sb & b.getCanBitsMask(bitsLen), true
|
||||
} else { //当前行没有有足够的bits
|
||||
sb1 := b.Data[b.readRowIndex]
|
||||
rt1 := sb1 >> (b.readColIndex - rowBitsLen + 1)
|
||||
rt1 <<= bitsLen - rowBitsLen
|
||||
b.toNextReadPos(rowBitsLen)
|
||||
if !b.isReadEnd() {
|
||||
readBitsLen := bitsLen - rowBitsLen
|
||||
if b.curRowCanReadLen() >= readBitsLen {
|
||||
sb2 := b.Data[b.readRowIndex]
|
||||
sb2 >>= b.readColIndex - readBitsLen + 1
|
||||
b.toNextReadPos(readBitsLen)
|
||||
return (rt1 | sb2) & b.getCanBitsMask(bitsLen), true
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//
|
||||
return 0, false
|
||||
}
|
||||
|
||||
// 是否读到最后一行
|
||||
func (b *CanBits) isReadInLastRow() bool {
|
||||
return b.readRowIndex == len(b.Data)-1
|
||||
}
|
||||
|
||||
// 当前可读行可读bits数
|
||||
func (b *CanBits) curRowCanReadLen() int {
|
||||
if b.isReadInLastRow() {
|
||||
return b.LastBitsLen - (7 - b.readColIndex)
|
||||
} else {
|
||||
return b.readColIndex + 1
|
||||
}
|
||||
}
|
||||
func (b *CanBits) toNextReadPos(hadReadRowLen int) {
|
||||
lastRow := b.isReadInLastRow()
|
||||
b.readColIndex -= hadReadRowLen
|
||||
if !lastRow {
|
||||
if b.readColIndex < 0 {
|
||||
b.readRowIndex++
|
||||
b.readColIndex = 7
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 是否读结束
|
||||
func (b *CanBits) isReadEnd() bool {
|
||||
return b.isReadInLastRow() && (7-b.readColIndex >= b.LastBitsLen) || b.readRowIndex < 0
|
||||
}
|
||||
|
||||
// AddByte 添加一整个字节bit数据
|
||||
func (b *CanBits) AddByte(bits byte) bool {
|
||||
return b.AddBits(bits, 8)
|
||||
}
|
||||
|
||||
// AddBits 添加bit数据，bitsLen-bits字节中有效位数，有效位向低位对齐
|
||||
func (b *CanBits) AddBits(bits byte, bitsLen int) bool {
|
||||
if bitsLen < 1 || bitsLen > 8 {
|
||||
return false
|
||||
}
|
||||
if len(b.Data) <= 0 {
|
||||
b.LastBitsLen = 8
|
||||
}
|
||||
//
|
||||
bits &= b.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
|
||||
}
|
||||
return true
|
||||
}
|
||||
func (b *CanBits) 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")
|
||||
}
|
||||
}
|
||||
|
||||
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)
|
||||
)
|
||||
|
||||
///////////////////// CRC ////////////////////////
|
||||
|
||||
const (
|
||||
CAN_CRC16_ATPREQ = 0x11021
|
||||
)
|
||||
|
||||
var (
|
||||
crc16AtpReqTable []uint32 = nil
|
||||
)
|
||||
|
||||
func CanCreateCrcTable() {
|
||||
if crc16AtpReqTable == nil {
|
||||
crc16AtpReqTable = CreateCrcTable(CAN_CRC16_ATPREQ, 16, false)
|
||||
}
|
||||
}
|
||||
func calculateAtpReqCrc16(data []byte) uint16 {
|
||||
crc := CrcTableBased(data, 16, 0, false, false, 0, crc16AtpReqTable)
|
||||
return uint16(crc)
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user