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

package message

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"math"
)

const (
	radar_head1 byte = 0x0C
	radar_head2 byte = 0xFC
)

var autoInc byte = 0
var (
	fixed_speed     = 0.009155
	driftDefaultVal = 0.1
)

// 雷达与VOBC接口-雷达通讯协议
type RadarInfo struct {
	AutoInc        byte   //自增计数器，每发送一次自增1.范围0-256
	RealSpeed      uint16 //实际速度
	DriftCounterS1 uint16 //位移计数器S1
	DriftCounterS2 uint16 //位移计数器S2
	InnerCheck1    byte   //内部使用，我们只有在协议效验时用到该两个字节
	InnerCheck2    byte   //内部使用，我们只有在协议效验时用到该两个字节
	State          *RadarState
	Tail           byte
}

func NewRadarSender(speed float32, forward bool, displacement uint16) *RadarInfo {
	state := &RadarState{SwModel1: 1, SwModel0: 1, calculateBit: 1, signalQualityBit: 1, BlackoutBit: 0, DirectionState: 1, Direction: IsTrue(forward)}
	ri := &RadarInfo{State: state}
	ri.RealSpeed = uint16(math.Abs(float64(speed*3.6) / fixed_speed))
	if float64(displacement/10) > 9999 {
		ri.DriftCounterS1 = 9999
	} else {
		ri.DriftCounterS1 = displacement / 10
	}
	if (float64(displacement) / 1000000) > 9999 {
		ri.DriftCounterS2 = 9999
	} else {
		ri.DriftCounterS2 = uint16(float64(displacement) / 1000000)
	}
	return ri
}

type RadarState struct {
	SwModel1         byte
	SwModel0         byte
	calculateBit     byte //计算状态位
	signalQualityBit byte //信号质量标志位
	BlackoutBit      byte //Blackout标志位
	innerBit         byte //内部使用
	DirectionState   byte //方向状态 1：行驶方向有效 	0：行驶方向无效
	Direction        byte //行驶方向 1：前向	0：反向

}

func (r *RadarState) Encode() byte {
	var state byte
	state = setBit(state, 7, r.SwModel1)
	state = setBit(state, 6, r.SwModel0)
	state = setBit(state, 5, r.calculateBit)
	state = setBit(state, 4, r.signalQualityBit)
	state = setBit(state, 3, r.BlackoutBit)
	state = setBit(state, 1, r.DirectionState)
	state = setBit(state, 0, r.Direction)
	return state
}

func (r RadarInfo) Encode() []byte {
	buf := make([]byte, 0)
	buf = append(buf, radar_head1)
	buf = append(buf, radar_head2)
	autoInc = autoInc + 1
	buf = append(buf, autoInc)

	buf = binary.LittleEndian.AppendUint16(buf, r.RealSpeed)
	buf = binary.LittleEndian.AppendUint16(buf, r.DriftCounterS1)
	buf = binary.LittleEndian.AppendUint16(buf, r.DriftCounterS2)

	buf = append(buf, 0)
	buf = append(buf, 0)

	buf = append(buf, r.State.Encode())
	var sum = 0
	for _, d := range buf {
		sum += int(d)
	}
	buf = append(buf, byte(^sum+1))
	return buf
}
func (r *RadarInfo) Decode(data []byte) error {
	if len(data) < 13 {
		return fmt.Errorf("雷达数据预读取失败，需要读取13字节，可读取:%v", len(data))
	}

	buf := bytes.NewBuffer(data)
	_, _, err := readHeader(buf)
	if err != nil {
		return err
	}
	autoInc := readByteData(buf)
	speed := readSpeedOrCounter(buf)
	s1 := readSpeedOrCounter(buf)
	s2 := readSpeedOrCounter(buf)
	i1, i2 := readRadarInnerData(buf)
	state := readRadarState(buf)
	tail := readByteData(buf)
	r.AutoInc = autoInc
	r.RealSpeed = speed
	r.DriftCounterS1 = s1
	r.DriftCounterS2 = s2
	r.InnerCheck1 = i1
	r.InnerCheck2 = i2
	r.State = state
	r.Tail = tail
	if !(r.Tail == r.createTail()) {
		return fmt.Errorf("数据解析完成，但协议效验不通过")
	}

	return nil
}
func (s *RadarState) Decode(d byte) {
	s.SwModel1 = GetBit(d, 7)
	s.SwModel0 = GetBit(d, 6)
	s.calculateBit = GetBit(d, 5)
	s.signalQualityBit = GetBit(d, 4)
	s.BlackoutBit = GetBit(d, 3)
	s.DirectionState = GetBit(d, 1)
	s.Direction = GetBit(d, 0)
}

func culDataSize(d uint16) int {
	return int(d>>8) + int(d&0x00FF)
}

func (r *RadarInfo) createTail() byte {
	var sum = int(radar_head1) + int(radar_head2)
	sum += int(r.AutoInc)
	sum += culDataSize(r.RealSpeed)
	sum += culDataSize(r.DriftCounterS1)
	sum += culDataSize(r.DriftCounterS2)
	sum += int(r.InnerCheck1)
	sum += int(r.InnerCheck2)
	sum += int(r.State.Encode())
	return byte(^sum + 1)
}

func readHeader(buf *bytes.Buffer) (byte, byte, error) {
	/*if buf.Len() < 2 {
		return 0, 0, fmt.Errorf("雷达协议解析头部没有可读充足的数据")
	}*/
	d1, _ := buf.ReadByte()
	d2, _ := buf.ReadByte()
	if d1 == radar_head1 && d2 == radar_head2 {
		return d1, d2, nil
	}
	return 0, 0, fmt.Errorf("雷达协议解析头部未找到对应的头部帧")
}

func readByteData(buf *bytes.Buffer) byte {
	/*	if buf.Len() < 1 {
		return 0, fmt.Errorf("")
	}*/
	d, _ := buf.ReadByte()
	return d
}
func readSpeedOrCounter(buf *bytes.Buffer) uint16 {
	ss, _ := buf.ReadByte()
	limit, _ := buf.ReadByte()
	data := binary.LittleEndian.Uint16([]byte{ss, limit})
	return data
	//return &RadarData{SourceData: ss, valRange: limit, data: data}
}

func readRadarInnerData(buf *bytes.Buffer) (byte, byte) {
	/*if buf.Len() < 2 {
		return 0, 0, fmt.Errorf("")
	}*/
	i1, _ := buf.ReadByte()
	i2, _ := buf.ReadByte()
	return i1, i2
}
func readRadarState(buf *bytes.Buffer) *RadarState {
	/*if buf.Len() < 1 {
		return nil, fmt.Errorf("")
	}*/
	state, _ := buf.ReadByte()
	s := &RadarState{}
	s.Decode(state)
	return s
}

func driftCounter(buf *bytes.Buffer) (uint16, error) {
	if buf.Len() < 2 {
		return 0, fmt.Errorf("")
	}
	var driftCounter uint16
	binary.Read(buf, binary.LittleEndian, &driftCounter)
	return driftCounter, nil
}