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

package message

import "fmt"

// 消息包头解析
type InterlockMsgPkgHeader struct {
	Header1      byte  //包头1	1个字节
	Header2      byte  //包头2	1个字节
	TypeCode     uint8 //类型码	1个字节 0x02
	SerialNumber uint8 //序列号	1个字节 序列号0~255
	Reserve1     byte  //预留	1个字节
	Reserve2     byte  //预留	1个字节
}

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 (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   // 包尾
}

var boolsToByteArrLen int = 8

func (m *InterlockSendMsgPkg) Encode() []byte {
	var data []byte
	data = append(data, m.Header.Encode()...)
	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:         &InterlockMsgPkgHeader{},
		Tail:           &InterlockMsgPkgTail{},
	}
}

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(t.DriveInfo, 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(r []bool, buf []byte, start, end int) {
	for i := start; i < end; i++ {
		b := buf[i]
		for bit := 7; bit >= 0; bit-- {
			r = append(r, (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
}