179 lines
4.8 KiB
Go
179 lines
4.8 KiB
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
|
||
}
|