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

package message

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

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

var autoInc = atomic.Int32{}

// 雷达与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
}

type RadarState struct {
	SourceState      byte   //原数据
	Model            string // 天线模式
	SyntheticalState string //综合状态
	DirState         string //方向状态
	Dir              string //方向
}

func (r RadarInfo) Encode() []byte {
	buf := make([]byte, 0)
	buf = append(buf, radar_head1)
	buf = append(buf, radar_head2)
	if tmp := autoInc.Add(1); tmp >= 256 {
		autoInc.Store(0)
		buf = append(buf, 0)
	} else {
		buf = append(buf, byte(tmp))
	}
	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)
	//6,7位 11
	//3,4,5位  011
	// 1位 1
	//0位 1
	buf = append(buf, 0|(byte(1)<<7)|(byte(1)<<6)|(byte(1)<<5)|(byte(1)<<4)|(byte(1)<<1)|(byte(1)<<0))
	var sum int = 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("数据解析完成，但协议效验不通过")
	}
	state.parseState()
	return nil
}
func (s *RadarState) parseState() {
	//第7位 == SW_Mode0, 第6位 == SW_Mode1
	// 11:两个天线和双通道都OK
	// 10：DRS05_Single-Mode 40度,50度的天线或通道故障
	// 01：DRS05_Single-Mode 50度,40度的天线或通道故障
	// 00：错误模式 双侧天线或通道都故障
	// 模式的工作差别：工作在11.模式时效果最好。单模式10或01时可信度下降。
	arr := s.getBitsStateArr()
	s.Model = bitStateStr(arr[6:])
	// 第5位=计算状态位,第4位=信号质量标志位,第3位=Black5out标志位
	// 110：计算状态，高质量
	// 地面信号反射良好，高精度的信号计算
	// 100： 计算状态，低质量
	// 地面信号反射一般，信号计算可能会有缺陷
	// 001： 处于Blackout 状态
	// 车辆行驶时无地面反射信号，无法进行信号计算
	// 000： 信号搜寻状态
	// 在整个频率范围内搜寻多普勒信号，此时的速度和位移信息无效，当频率搜寻正确后，会转为计算模式。
	s.SyntheticalState = bitStateStr(arr[3:6])
	//  第1位 =方向状态
	// 1：行驶方向有效
	// 0：行驶方向无效
	s.DirState = bitStateStr(arr[1:2])
	//  第0位 =行驶方向
	// 1：前向
	// 0：反向
	s.Dir = bitStateStr(arr[0:1])
}

func (s *RadarState) getBitsStateArr() [8]byte {
	//bits := make([]byte, 8)
	var bits2 [8]byte = [8]byte{}
	for i := 0; i < 8; i++ {
		bit := s.SourceState >> uint(i) & 1
		bits2[i] = bit
	}
	return bits2
}

func bitStateStr(data []byte) string {
	var build = strings.Builder{}
	for i := len(data) - 1; i >= 0; i-- {
		build.WriteString(fmt.Sprintf("%v", data[i]))
	}
	/*for _, d := range data {
		build.WriteString(fmt.Sprintf("%v", d))
	}*/
	return build.String()
}
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.SourceState)
	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()
	return &RadarState{SourceState: 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
}