rts-sim-testing-service/third_party/message/interlock.go

package message

import (
	"fmt"
	"math"
)

// 消息包头解析
type interlockMsgPkgHeader struct {
	header1      byte  //包头1	1个字节 0xFF
	header2      byte  //包头2	1个字节 0xFF
	typeCode     uint8 //类型码	1个字节 0x02
	serialNumber uint8 //序列号	1个字节 序列号0~255
	reserve1     byte  //预留	1个字节
	reserve2     byte  //预留	1个字节
}

func newInterlockMsgPkgHeader() *interlockMsgPkgHeader {
	return &interlockMsgPkgHeader{header1: 0xFF, header2: 0xFF, typeCode: 0x02}
}

func (h *interlockMsgPkgHeader) encode() []byte {
	return []byte{h.header1, h.header2, h.typeCode, h.serialNumber, h.reserve1, h.reserve2}
}

func (h *interlockMsgPkgHeader) decode(buf []byte) error {
	if len(buf) < 6 {
		return fmt.Errorf("数据少于6个字节")
	}
	h.header1 = buf[0]
	h.header2 = buf[1]
	h.typeCode = buf[2]
	h.serialNumber = buf[3]
	h.reserve1 = buf[4]
	h.reserve2 = buf[5]
	return nil
}

// 消息包尾解析
type interlockMsgPkgTail struct {
	tail []byte // 包尾2个字节
}

func newInterlockMsgPkgTail() *interlockMsgPkgTail {
	return &interlockMsgPkgTail{tail: make([]byte, 2)}
}

func (t *interlockMsgPkgTail) encode() []byte {
	if len(t.tail) == 0 {
		return make([]byte, 2)
	}
	return t.tail
}

func (t *interlockMsgPkgTail) decode(buf []byte) error {
	if len(buf) < 2 {
		return fmt.Errorf("数据少于2个字节")
	}
	t.tail = buf[len(buf)-2:]
	return nil
}

// 发送给联锁的采集数据
type InterlockSendMsgPkg struct {
	header *interlockMsgPkgHeader // 包头
	info   []bool                 // 发给联锁的状态数据
	tail   *interlockMsgPkgTail   // 包尾
}

func NewInterlockSendMsgPkg(info []bool) *InterlockSendMsgPkg {
	return &InterlockSendMsgPkg{
		header: newInterlockMsgPkgHeader(),
		info:   info,
		tail:   newInterlockMsgPkgTail(),
	}
}

var boolsToByteArrLen int = 8

func (m *InterlockSendMsgPkg) SetSerialNumber(serialNumber uint8) {
	m.header.serialNumber = serialNumber
}

func (m *InterlockSendMsgPkg) Encode() []byte {
	var data []byte
	data = append(data, m.header.encode()...)
	bitLen := len(m.info)
	byteLen := int(math.Ceil(float64(bitLen) / 8))
	infoBytes := make([]byte, byteLen)
	for i, b := range m.info {
		if b {
			infoBytes[i/8] = infoBytes[i/8] + (1 << (i % 8))
		}
	}
	data = append(data, infoBytes...)

	//for index, length, cycles := 0, len(m.info), len(m.info)/boolsToByteArrLen; index < cycles; index++ {
	//	startIndex := index * boolsToByteArrLen
	//	toByteArr := [8]bool{}
	//	for i := 0; i < boolsToByteArrLen && startIndex < length; i++ {
	//		startIndex = startIndex + i
	//		toByteArr[i] = m.info[startIndex+i]
	//	}
	//	data = append(data, boolsToByte(toByteArr))
	//}
	data = append(data, m.tail.encode()...)
	return data
}

// bool数组转byte
func boolsToByte(flags [8]bool) byte {
	var result uint8
	for index, b := range flags {
		if b {
			result = result + (1 << index)
		}
	}
	return result
}

// 收到联锁发来的驱动数据
type InterlockReceiveMsgPkg struct {
	toagent_len    int
	et_out_num     int
	tcc_output_len int
	header         *interlockMsgPkgHeader      // 包头
	SyncZone       []byte                      // 同步区状态
	DriveInfo      []bool                      // 驱动数据
	TccInfo        []*InterlockResponderMsgPkg // 应答器报文
	tail           *interlockMsgPkgTail        // 包尾
}

// 应答器数据格式
type InterlockResponderMsgPkg struct {
	Index         byte
	InternalIndex byte
	Reserve       byte
	MsgInfo       []byte
}

// ET_OUT_NUM、TOAGENTLEN、TCC_OUTPUT_LEN（应答器数量*131）的具体数值取决于数据配置。
func NewInterlockReceiveMsgPkg(tlen, etLen, tccLen int) *InterlockReceiveMsgPkg {
	return &InterlockReceiveMsgPkg{
		toagent_len:    tlen,
		et_out_num:     etLen,
		tcc_output_len: tccLen * 131,
		header:         newInterlockMsgPkgHeader(),
		tail:           newInterlockMsgPkgTail(),
	}
}

func (t *InterlockReceiveMsgPkg) Decode(buf []byte) error {
	if len(buf) < 8 {
		return fmt.Errorf("buf 没有足够的数据")
	}
	var preIndex, lastIndex int = 0, 6
	// 包头
	t.header.decode(buf[preIndex:lastIndex])
	// 同步区状态
	preIndex = lastIndex
	lastIndex = lastIndex + t.toagent_len
	t.SyncZone = buf[preIndex:lastIndex]
	// 驱动数据
	preIndex = lastIndex
	lastIndex = lastIndex + t.et_out_num
	t.parseByte(buf, preIndex, lastIndex)
	// 应答器报文
	preIndex = lastIndex
	lastIndex = lastIndex + t.tcc_output_len
	t.TccInfo = parseResponder(buf, preIndex, lastIndex)
	// 包尾
	t.tail.decode(buf)
	return nil
}

func (t *InterlockReceiveMsgPkg) parseByte(buf []byte, start, end int) {
	for i := start; i < end; i++ {
		b := buf[i]
		for bit := 7; bit >= 0; bit-- {
			t.DriveInfo = append(t.DriveInfo, (b&(1<<bit)) != 0)
		}
	}
}

func parseResponder(buf []byte, start, end int) []*InterlockResponderMsgPkg {
	var msgs []*InterlockResponderMsgPkg
	for i := start; i < end; i = i + 128 {
		b := buf[i : i+128]
		msgs = append(msgs, &InterlockResponderMsgPkg{
			Index:         b[0],
			InternalIndex: b[1],
			Reserve:       b[2],
			MsgInfo:       b[3:],
		})
	}
	return msgs
}