From 5e76894404a145d247008adc126ed2f78d91539d Mon Sep 17 00:00:00 2001 From: tiger_zhou Date: Mon, 22 Jan 2024 15:08:46 +0800 Subject: [PATCH] =?UTF-8?q?=E9=9B=B7=E8=BE=BE=E4=B8=8EVOBC=E5=8D=8F?= =?UTF-8?q?=E8=AE=AE=E8=A7=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- go.mod | 2 +- third_party/message/radar.go | 178 +++++++++++++++++++++++++++++++++++ 2 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 third_party/message/radar.go diff --git a/go.mod b/go.mod index dee3e0b..7d2dd3d 100644 --- a/go.mod +++ b/go.mod @@ -9,6 +9,7 @@ require ( github.com/golang/protobuf v1.5.3 github.com/google/uuid v1.4.0 github.com/sagikazarmark/slog-shim v0.1.0 + github.com/snksoft/crc v1.1.0 github.com/spf13/viper v1.18.1 github.com/stretchr/testify v1.8.4 github.com/swaggo/files v1.0.1 @@ -47,7 +48,6 @@ require ( github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 // indirect github.com/sagikazarmark/locafero v0.4.0 // indirect github.com/simonvetter/modbus v1.6.0 // indirect - github.com/snksoft/crc v1.1.0 // indirect github.com/sourcegraph/conc v0.3.0 // indirect github.com/yohamta/donburi v1.3.9 // indirect go.uber.org/atomic v1.9.0 // indirect diff --git a/third_party/message/radar.go b/third_party/message/radar.go new file mode 100644 index 0000000..89da2b7 --- /dev/null +++ b/third_party/message/radar.go @@ -0,0 +1,178 @@ +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 +}