212 lines
5.7 KiB
Go
212 lines
5.7 KiB
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
|
||
}
|