From 5ee14ba52e9ac236c1baa08e76093eaef9a0b040 Mon Sep 17 00:00:00 2001 From: tiger_zhou Date: Wed, 24 Jan 2024 17:12:34 +0800 Subject: [PATCH] =?UTF-8?q?=E9=9B=B7=E8=BE=BE=E5=8F=91=E9=80=81=E5=8A=9F?= =?UTF-8?q?=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- bj-rtss-message | 2 +- config/config.go | 8 +- example/mock_train_service/main.go | 6 +- third_party/message/radar.go | 84 +++++++++++----- third_party/message/radar_test.go | 47 --------- third_party/radar/radar_test.go | 28 ++++++ third_party/radar/radar_vobc.go | 97 +++++++++++++++++++ .../wayside/memory/wayside_simulation.go | 25 +++++ ts/test_simulation_manage.go | 5 + 9 files changed, 226 insertions(+), 76 deletions(-) delete mode 100644 third_party/message/radar_test.go create mode 100644 third_party/radar/radar_test.go create mode 100644 third_party/radar/radar_vobc.go diff --git a/bj-rtss-message b/bj-rtss-message index 7b637f0..e800ded 160000 --- a/bj-rtss-message +++ b/bj-rtss-message @@ -1 +1 @@ -Subproject commit 7b637f0f519e0c6f5fa6817546f70000ac22fd10 +Subproject commit e800ded2c3b80a73cab3efc07c7146b7ae48035a diff --git a/config/config.go b/config/config.go index 8d975d5..9064446 100644 --- a/config/config.go +++ b/config/config.go @@ -69,8 +69,14 @@ type ThridPartyConfig struct { ElectricMachinery ElectricMachineryConfig `json:"electricMachinery" description:"电机配置"` BtmCanet BtmCanetConfig `json:"btmCanet" description:"BTM关联的网关设备CANET配置"` CidcModbus []CidcModbusConfig `json:"cidcModbus" description:"联锁驱采Modbus接口配置"` + Radar RadarConfig `json:"radar" description:"车载雷达相关配置"` +} +type RadarConfig struct { + Open bool `json:"open" description:"是否开启"` + RemoteIp string `json:"remoteIp" description:"远端接收列车信息ip"` + RemotePort uint32 `json:"remotePort" description:"远端接收列车信息端口"` + //LocalPort uint32 `json:"localPort" description:"本地端口"` } - type CidcModbusConfig struct { Open bool `json:"open" description:"是否开启"` Url string `json:"url" description:"接口URL【格式tcp://{ip}:{port}】" default:"tcp://127.0.0.1:502"` // 连接地址 diff --git a/example/mock_train_service/main.go b/example/mock_train_service/main.go index 9a670e7..9a8c70b 100644 --- a/example/mock_train_service/main.go +++ b/example/mock_train_service/main.go @@ -21,8 +21,10 @@ var ti = &ReceiveTrainInfo{ var trainRun bool -var IP = net.IPv4(192, 168, 3, 7) -var SendIP = net.IPv4(192, 168, 3, 7) +// var IP = net.IPv4(192, 168, 3, 7) +// var SendIP = net.IPv4(192, 168, 3, 7) +var IP = net.IPv4(127, 0, 0, 1) +var SendIP = net.IPv4(127, 0, 0, 1) func main() { listen, err := net.ListenUDP("udp", &net.UDPAddr{ diff --git a/third_party/message/radar.go b/third_party/message/radar.go index ee970cc..951449e 100644 --- a/third_party/message/radar.go +++ b/third_party/message/radar.go @@ -5,6 +5,7 @@ import ( "encoding/binary" "fmt" "strings" + "sync/atomic" ) const ( @@ -12,22 +13,20 @@ const ( radar_head2 byte = 0xFC ) +var autoInc = atomic.Int32{} + // 雷达与VOBC接口-雷达通讯协议 -type Radar struct { - AutoInc byte //自增计数器，每发送一次自增1.范围0-256 - RealSpeed *RadarData //实际速度 - DriftCounterS1 *RadarData //位移计数器S1 - DriftCounterS2 *RadarData //位移计数器S1 - InnerCheck1 byte //内部使用，我们只有在协议效验时用到该两个字节 - InnerCheck2 byte //内部使用，我们只有在协议效验时用到该两个字节 +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 RadarData struct { - SourceData byte //接收源数据 - data uint16 //移位后的数据 - valRange byte //数据取值范围 -} + type RadarState struct { SourceState byte //原数据 Model string // 天线模式 @@ -36,7 +35,34 @@ type RadarState struct { Dir string //方向 } -func (r *Radar) Decode(data []byte) error { +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)) } @@ -61,15 +87,12 @@ func (r *Radar) Decode(data []byte) error { r.InnerCheck2 = i2 r.State = state r.Tail = tail - if !r.checkTail() { + if !(r.Tail == r.createTail()) { return fmt.Errorf("数据解析完成，但协议效验不通过") } state.parseState() return nil } -func (rd *RadarData) getSumVal() byte { - return rd.SourceData + rd.valRange -} func (s *RadarState) parseState() { //第7位 == SW_Mode0, 第6位 == SW_Mode1 // 11:两个天线和双通道都OK @@ -119,10 +142,22 @@ func bitStateStr(data []byte) string { }*/ 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.SourceState) - return r.Tail == byte(^sum+1) +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("雷达协议解析头部没有可读充足的数据") @@ -142,13 +177,12 @@ func readByteData(buf *bytes.Buffer) byte { d, _ := buf.ReadByte() return d } -func readSpeedOrCounter(buf *bytes.Buffer) *RadarData { - /* if buf.Len() < 2 { - return nil, fmt.Errorf("") - }*/ +func readSpeedOrCounter(buf *bytes.Buffer) uint16 { ss, _ := buf.ReadByte() limit, _ := buf.ReadByte() - return &RadarData{SourceData: ss, valRange: limit, data: uint16(ss) << 8} + data := binary.LittleEndian.Uint16([]byte{ss, limit}) + return data + //return &RadarData{SourceData: ss, valRange: limit, data: data} } func readRadarInnerData(buf *bytes.Buffer) (byte, byte) { diff --git a/third_party/message/radar_test.go b/third_party/message/radar_test.go deleted file mode 100644 index 519db6c..0000000 --- a/third_party/message/radar_test.go +++ /dev/null @@ -1,47 +0,0 @@ -package message - -import ( - "fmt" - "testing" -) - -func TestRadar(t *testing.T) { - data := make([]byte, 0) - data = append(data, radar_head1) //0 - data = append(data, radar_head2) //1 - data = append(data, 1) //自增 - data = append(data, 1) //速度3 - data = append(data, 0) //4 - data = append(data, 1) //s1 5 - data = append(data, 0) //6 - data = append(data, 1) //s2 7 - data = append(data, 0) //8 - data = append(data, 0) //内部使用 9 - data = append(data, 0) //内部使用 9 - data = append(data, createState()) - var sum int - for _, s := range data { - sum += int(s) - } - data = append(data, byte(^sum+1)) - radar := &Radar{} - radar.Decode(data) - fmt.Println(radar) -} - -func createState() byte { - var b byte = 0 - b1 := b | (byte(1) << 7) | (byte(1) << 6) | (byte(1) << 5) | (byte(1) << 4) | (byte(1) << 1) | (byte(1) << 1) - return b1 -} -func TestR1(t *testing.T) { - var b byte = 0 - fmt.Printf("%08b\n", b) - //6,7位 11 - //3,4,5位 011 - // 1位 1 - //0位 1 - b1 := b | (byte(1) << 7) | (byte(1) << 6) | (byte(1) << 5) | (byte(1) << 4) | (byte(1) << 1) | (byte(1) << 0) - fmt.Printf("%08b\n", b1) - fmt.Println(b1) -} diff --git a/third_party/radar/radar_test.go b/third_party/radar/radar_test.go new file mode 100644 index 0000000..a297bd9 --- /dev/null +++ b/third_party/radar/radar_test.go @@ -0,0 +1,28 @@ +package radar + +import ( + "encoding/json" + "fmt" + "joylink.club/bj-rtsts-server/third_party/message" + "joylink.club/bj-rtsts-server/third_party/udp" + "testing" +) + +func TestUdp(t *testing.T) { + fmt.Println("准备启动服务...") + addr := fmt.Sprintf("%v:%v", "127.0.0.1", "8899") + server := udp.NewServer(addr, handle) + server.Listen() + for { + } +} + +func handle(d []byte) { + ri := message.RadarInfo{} + err := ri.Decode(d) + if err == nil { + + jsonD, _ := json.Marshal(ri) + fmt.Println(string(jsonD)) + } +} diff --git a/third_party/radar/radar_vobc.go b/third_party/radar/radar_vobc.go new file mode 100644 index 0000000..5c24d6d --- /dev/null +++ b/third_party/radar/radar_vobc.go @@ -0,0 +1,97 @@ +package radar + +import ( + "context" + "fmt" + "joylink.club/bj-rtsts-server/config" + "joylink.club/bj-rtsts-server/dto/state_proto" + "joylink.club/bj-rtsts-server/third_party/message" + "joylink.club/bj-rtsts-server/third_party/udp" + "log/slog" + "math" + "sync" + "time" +) + +type RadarVobc interface { + Start(radar RadarVobcManager) + Stop() + SendRadarInfo(ctx context.Context) +} + +type RadarVobcManager interface { + GetRunRadarConfig() config.RadarConfig + FindRadarTrain() *state_proto.TrainState +} + +const radar_interval = 15 + +var ( + initLock = sync.Mutex{} + _radar *radarVobc + fixed_speed = 0.009155 +) + +func Default() RadarVobc { + defer func() { + initLock.Unlock() + }() + initLock.Lock() + if _radar == nil { + _radar = &radarVobc{} + } + return _radar +} + +type radarVobc struct { + radarVobcTaskContext context.CancelFunc + vobcClient udp.UdpClient + radarVobcManager RadarVobcManager +} + +func (rv *radarVobc) Start(radar RadarVobcManager) { + config := radar.GetRunRadarConfig() + if config.RemoteIp == "" || config.RemotePort == 0 || !config.Open { + slog.Info("雷达未开启", "远端ip:", config.RemoteIp, "远端端口:", config.RemotePort, "是否开启:", config.Open) + return + } + + rv.vobcClient = udp.NewClient(fmt.Sprintf("%v:%v", config.RemoteIp, config.RemotePort)) + ctx, cancleFunc := context.WithCancel(context.Background()) + rv.radarVobcTaskContext = cancleFunc + rv.radarVobcManager = radar + go rv.SendRadarInfo(ctx) +} + +func (rv *radarVobc) SendRadarInfo(ctx context.Context) { + /*defer func() { + slog.Error("") + }()*/ + for { + select { + case <-ctx.Done(): + return + default: + + } + trainStatus := rv.radarVobcManager.FindRadarTrain() + if trainStatus != nil { + hourSpeed := float64(trainStatus.DynamicState.Speed / 100) + ri := message.RadarInfo{RealSpeed: uint16(math.Round(hourSpeed / fixed_speed)), DriftCounterS1: 0, DriftCounterS2: 0} + rv.vobcClient.SendMsg(ri) + time.Sleep(time.Millisecond * radar_interval) + } + } + +} +func (rv *radarVobc) Stop() { + if rv.vobcClient != nil { + rv.vobcClient.Close() + rv.vobcClient = nil + } + if rv.radarVobcTaskContext != nil { + rv.radarVobcTaskContext() + rv.radarVobcTaskContext = nil + } + +} diff --git a/ts/simulation/wayside/memory/wayside_simulation.go b/ts/simulation/wayside/memory/wayside_simulation.go index ef69783..4a19312 100644 --- a/ts/simulation/wayside/memory/wayside_simulation.go +++ b/ts/simulation/wayside/memory/wayside_simulation.go @@ -535,6 +535,31 @@ func (s *VerifySimulation) initRepository() error { return nil } +// GetRunRadarConfig 获取雷达配置信息 +func (s *VerifySimulation) GetRunRadarConfig() config.RadarConfig { + return s.runConfig.Radar +} + +// FindRadarTrain 查找一个列车 只有1端雷达开启啊 +func (s *VerifySimulation) FindRadarTrain() *state_proto.TrainState { + var trainStatus *state_proto.TrainState + s.Memory.Status.TrainStateMap.Range(func(k any, v any) bool { + val, ok := v.(*state_proto.TrainState) + if ok { + if val.TrainEndsA.RadarEnable && val.TrainEndsB.RadarEnable { + //trainStatus = val + //return false + return true + } else if val.TrainEndsA.RadarEnable || val.TrainEndsB.RadarEnable { + trainStatus = val + return false + } + } + return true + }) + return trainStatus +} + func buildProtoRepository(mapIds []int32) (*proto.Repository, error) { repo := &proto.Repository{} var exceptStationGiMapIds []int32 diff --git a/ts/test_simulation_manage.go b/ts/test_simulation_manage.go index 123cdd0..c23a051 100644 --- a/ts/test_simulation_manage.go +++ b/ts/test_simulation_manage.go @@ -2,6 +2,7 @@ package ts import ( "fmt" + "joylink.club/bj-rtsts-server/third_party/radar" "log/slog" "runtime" "strconv" @@ -130,6 +131,8 @@ func runThirdParty(s *memory.VerifySimulation) error { if err != nil { return err } + //列车雷达发送vobc + radar.Default().Start(s) return nil } @@ -151,6 +154,8 @@ func stopThirdParty(s *memory.VerifySimulation) { can_btm.Default().Stop() // 联锁驱采Modbus服务停止 cidcmodbus.Stop() + // 雷达服务停止 + radar.Default().Stop() } func createSimulationId(projectId int32) string {