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

package message

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

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

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

func (r *Radar) 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.checkTail() {
		return fmt.Errorf("数据解析完成，但协议效验不通过")
	}
	return nil
}
func (rd *RadarData) getSumVal() byte {
	return rd.sourceData + rd.valRange
}

// GetModel 第6位 == SW_Mode0,第7位 == SW_Mode1
// 11:两个天线和双通道都OK
// 10：DRS05_Single-Mode 40度,50度的天线或通道故障
// 01：DRS05_Single-Mode 50度,40度的天线或通道故障
// 00：错误模式 双侧天线或通道都故障
// 模式的工作差别：工作在11.模式时效果最好。单模式10或01时可信度下降。
func (s *RadarState) GetModel() string {
	arr := s.getBitsStateArr()
	return bitStateStr(arr[6:])
}

// GetSyntheticalState 第3位=计算状态位,第4位=信号质量标志位,第5位=Black5out标志位
// 110：计算状态，高质量
// 地面信号反射良好，高精度的信号计算
// 100： 计算状态，低质量
// 地面信号反射一般，信号计算可能会有缺陷
// 001： 处于Blackout 状态
// 车辆行驶时无地面反射信号，无法进行信号计算
// 000： 信号搜寻状态
// 在整个频率范围内搜寻多普勒信号，此时的速度和位移信息无效，当频率搜寻正确后，会转为计算模式。
func (s *RadarState) GetSyntheticalState() string {
	arr := s.getBitsStateArr()
	return bitStateStr(arr[3:6])
}

// GetDirState 第1位 =方向状态
// 1：行驶方向有效
// 0：行驶方向无效
func (s *RadarState) GetDirState() string {
	arr := s.getBitsStateArr()
	return bitStateStr(arr[1:2])
}

// GetDir 第0位 =行驶方向
// 1：前向
// 0：反向
func (s *RadarState) GetDir() string {
	arr := s.getBitsStateArr()
	return bitStateStr(arr[0:1])
}
func (s *RadarState) getBitsStateArr() []byte {
	bits := make([]byte, 8)
	for i := 0; i < 8; i++ {
		bit := s.State >> uint(i) & 1
		bits[i] = bit
	}
	return bits
}

func bitStateStr(data []byte) string {
	var build = strings.Builder{}
	for _, d := range data {
		build.WriteString(fmt.Sprintf("%v", d))
	}
	return build.String()
}
func (r *Radar) checkTail() bool {
	var sum = int(radar_head1) + int(radar_head2) + int(r.AutoInc+r.RealSpeed.getSumVal()+r.DriftCounterS1.getSumVal()+r.DriftCounterS2.getSumVal()+r.InnerCheck1+r.InnerCheck2+r.State.State)
	return r.Tail == 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 0, 0, fmt.Errorf("雷达协议解析头部未找到对应的头部帧")
	}
	return d1, d2, nil
}

func readByteData(buf *bytes.Buffer) byte {
	/*	if buf.Len() < 1 {
		return 0, fmt.Errorf("")
	}*/
	d, _ := buf.ReadByte()
	return d
}
func readSpeedOrCounter(buf *bytes.Buffer) *RadarData {
	/*	if buf.Len() < 2 {
		return nil, fmt.Errorf("")
	}*/
	ss, _ := buf.ReadByte()
	limit, _ := buf.ReadByte()
	return &RadarData{sourceData: ss, valRange: limit}
}

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()
	return &RadarState{State: state}
}

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
}