diff --git a/ats/verify/simulation/simulation_manage.go b/ats/verify/simulation/simulation_manage.go index f3d9712..7dc735b 100644 --- a/ats/verify/simulation/simulation_manage.go +++ b/ats/verify/simulation/simulation_manage.go @@ -13,6 +13,7 @@ import ( "joylink.club/bj-rtsts-server/ats/verify/protos/state" "joylink.club/bj-rtsts-server/config" "joylink.club/bj-rtsts-server/dynamics" + "joylink.club/bj-rtsts-server/vobc" "joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory" "joylink.club/bj-rtsts-server/dto" @@ -36,6 +37,9 @@ var simulationId_prefix = (func() string { })() func init() { + vobc.RegisterTrainInfoHandler(func(info *vobc.ReceiveTrainInfo) { + zap.S().Debug("接到列车信息", info) + }) dynamics.RegisterTrainInfoHandler(func(info *dynamics.TrainInfo) { for _, simulation := range GetSimulationArr() { sta, ok := simulation.Memory.Status.TrainStateMap.Load(strconv.Itoa(int(info.Number))) diff --git a/config/config.go b/config/config.go index 4f8946c..17c1145 100644 --- a/config/config.go +++ b/config/config.go @@ -15,6 +15,7 @@ type AppConfig struct { Logging log Messaging messaging Dynamics dynamics + Vobc vobc } type server struct { Port int @@ -49,6 +50,11 @@ type dynamics struct { UdpRemotePort int HttpPort int } +type vobc struct { + Ip string + LocalPort int + RemotePort int +} var Config AppConfig diff --git a/config/dev.yml b/config/dev.yml index 6d1c9fa..75d0529 100644 --- a/config/dev.yml +++ b/config/dev.yml @@ -10,6 +10,11 @@ dynamics: udpLocalPort: 4000 udpRemotePort: 3000 httpPort: 7800 +# VOBC +vobc: + ip: 10.60.1.52 + localPort: 10000 + remotePort: 4000 # 数据源 datasource: diff --git a/config/test_local.yml b/config/test_local.yml index 61bfe72..5415e6f 100644 --- a/config/test_local.yml +++ b/config/test_local.yml @@ -10,6 +10,11 @@ dynamics: udpLocalPort: 4000 udpRemotePort: 3000 httpPort: 7800 +# VOBC +vobc: + ip: 10.60.1.52 + localPort: 10000 + remotePort: 4000 # 数据源 datasource: diff --git a/dynamics/udp.go b/dynamics/udp.go index de181d3..0ddfcb8 100644 --- a/dynamics/udp.go +++ b/dynamics/udp.go @@ -5,18 +5,19 @@ import ( "encoding/binary" "errors" "fmt" - "github.com/panjf2000/gnet/v2" - "go.uber.org/zap" - "joylink.club/bj-rtsts-server/config" "math" "net" "sync" "time" + + "github.com/panjf2000/gnet/v2" + "go.uber.org/zap" + "joylink.club/bj-rtsts-server/config" ) func init() { go func() { - for true { + for { info := <-trainInfoChan for e := handlerList.Front(); e != nil; e = e.Next() { func() { @@ -52,25 +53,57 @@ var ( trainInfoChan chan *TrainInfo = make(chan *TrainInfo) ) -func Run(tiFunc TurnoutInfoFunc) error { - mutex.Lock() - defer mutex.Unlock() - trainLifeSignalInit = false - return runSendTurnoutStateTask(tiFunc) -} - -func Stop() { - mutex.Lock() - defer mutex.Unlock() - turnoutInfoFunc = nil -} - var handlerList list.List type TrainInfoHandler func(info *TrainInfo) -func RegisterTrainInfoHandler(handler TrainInfoHandler) { - handlerList.PushBack(handler) +type udpServer struct { + gnet.BuiltinEventEngine + + eng gnet.Engine + addr string + multicore bool + + eventHandlers []gnet.EventHandler +} + +func (server *udpServer) OnBoot(eng gnet.Engine) gnet.Action { + server.eng = eng + zap.S().Infof("udp server with multi-core=%t is listening on %s\n", server.multicore, server.addr) + return gnet.None +} + +// OnTraffic 接收到数据后的解析 +func (server *udpServer) OnTraffic(c gnet.Conn) gnet.Action { + defer func() { + if r := recover(); r != nil { + zap.L().Error("udp服务数据解析异常", zap.Any("panic", r)) + } + }() + buf, _ := c.Next(-1) + lifeSignal := binary.BigEndian.Uint16(buf[0:2]) + if !trainLifeSignalInit { + trainLifeSignalInit = true + } else if trainLifeSignal < limit { + if lifeSignal < trainLifeSignal || lifeSignal > trainLifeSignal-limit { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, trainLifeSignal) + return gnet.None + } + } else if trainLifeSignal < math.MaxUint16-10000 { + if lifeSignal < trainLifeSignal { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, trainLifeSignal) + return gnet.None + } + } else { + if lifeSignal < trainLifeSignal && lifeSignal > trainLifeSignal+10000 { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, trainLifeSignal) + return gnet.None + } + } + trainLifeSignal = lifeSignal + trainInfo := decoderDynamicsTrainInfo(buf) + trainInfoChan <- trainInfo + return gnet.None } func RunUdpServer() { @@ -79,8 +112,26 @@ func RunUdpServer() { zap.L().Fatal("udp服务启动失败", zap.Error(err)) } -// sendTurnoutInfo 发送道岔信息 -func sendTurnoutInfo(info *TurnoutInfo) error { +func Run(tiFunc TurnoutInfoFunc) error { + mutex.Lock() + defer mutex.Unlock() + trainLifeSignalInit = false + return runSendTurnoutStateTask(tiFunc) +} + +// 注册数据操作 +func RegisterTrainInfoHandler(handler TrainInfoHandler) { + handlerList.PushBack(handler) +} + +func Stop() { + mutex.Lock() + defer mutex.Unlock() + turnoutInfoFunc = nil +} + +// 动力学消息发送消息 +func sendDynamicsMsg(buf []byte) error { defer func() { if r := recover(); r != nil { zap.S().Error("发送道岔信息失败", r) @@ -99,94 +150,11 @@ func sendTurnoutInfo(info *TurnoutInfo) error { zap.S().Error(err) } }(conn) - var data []byte - data = binary.BigEndian.AppendUint16(data, info.lifeSignal) - data = binary.BigEndian.AppendUint16(data, info.Code) - var b byte - if info.NPosition { - b |= 1 << 7 - } - if info.RPosition { - b |= 1 << 6 - } - data = append(data, b) - _, err = conn.Write(data) - + _, err = conn.Write(buf) return err } -type udpServer struct { - gnet.BuiltinEventEngine - - eng gnet.Engine - addr string - multicore bool - - eventHandlers []gnet.EventHandler -} - -func (server *udpServer) OnBoot(eng gnet.Engine) gnet.Action { - server.eng = eng - fmt.Printf("udp server with multi-core=%t is listening on %s\n", server.multicore, server.addr) - zap.S().Infof("udp server with multi-core=%t is listening on %s\n", server.multicore, server.addr) - return gnet.None -} - -// OnTraffic 接收到数据后的解析 -func (server *udpServer) OnTraffic(c gnet.Conn) gnet.Action { - defer func() { - if r := recover(); r != nil { - zap.L().Error("udp服务数据解析异常", zap.Any("panic", r)) - } - }() - buf, _ := c.Next(-1) - trainInfo := TrainInfo{} - trainInfo.LifeSignal = binary.BigEndian.Uint16(buf[0:2]) - if !trainLifeSignalInit { - trainLifeSignalInit = true - trainLifeSignal = trainInfo.LifeSignal - } else if trainLifeSignal < limit { - if trainInfo.LifeSignal < trainLifeSignal || trainInfo.LifeSignal > trainLifeSignal-limit { - zap.S().Debugf("丢弃列车信息[%d-%d]", trainInfo.LifeSignal, trainLifeSignal) - return gnet.None - } - } else if trainLifeSignal < math.MaxUint16-10000 { - if trainInfo.LifeSignal < trainLifeSignal { - zap.S().Debugf("丢弃列车信息[%d-%d]", trainInfo.LifeSignal, trainLifeSignal) - return gnet.None - } - } else { - if trainInfo.LifeSignal < trainLifeSignal && trainInfo.LifeSignal > trainLifeSignal+10000 { - zap.S().Debugf("丢弃列车信息[%d-%d]", trainInfo.LifeSignal, trainLifeSignal) - return gnet.None - } - } - trainLifeSignal = trainInfo.LifeSignal - trainInfo.Number = buf[2] - trainInfo.Len = binary.BigEndian.Uint16(buf[3:5]) - trainInfo.Link = buf[5] - trainInfo.LinkOffset = binary.BigEndian.Uint32(buf[6:10]) - trainInfo.Slope = binary.BigEndian.Uint16(buf[10:12]) - b := buf[12] - trainInfo.UpSlope = (b & (1 << 7)) != 0 - trainInfo.Up = (b & (1 << 6)) != 0 - trainInfo.TotalResistance = binary.BigEndian.Uint32(buf[14:18]) - trainInfo.AirResistance = binary.BigEndian.Uint32(buf[18:22]) - trainInfo.SlopeResistance = binary.BigEndian.Uint32(buf[22:26]) - trainInfo.CurveResistance = binary.BigEndian.Uint32(buf[26:30]) - trainInfo.Speed = math.Float32frombits(binary.BigEndian.Uint32(buf[30:34])) - trainInfo.HeadSpeed1 = math.Float32frombits(binary.BigEndian.Uint32(buf[34:38])) - trainInfo.HeadSpeed2 = math.Float32frombits(binary.BigEndian.Uint32(buf[38:42])) - trainInfo.TailSpeed1 = math.Float32frombits(binary.BigEndian.Uint32(buf[42:46])) - trainInfo.TailSpeed2 = math.Float32frombits(binary.BigEndian.Uint32(buf[46:50])) - trainInfo.HeadRadarSpeed = math.Float32frombits(binary.BigEndian.Uint32(buf[50:54])) - trainInfo.TailRadarSpeed = math.Float32frombits(binary.BigEndian.Uint32(buf[54:58])) - - trainInfoChan <- &trainInfo - - return gnet.None -} - +// 发送道岔状态任务 func runSendTurnoutStateTask(tiFunc TurnoutInfoFunc) error { if running { return nil @@ -209,7 +177,8 @@ func runSendTurnoutStateTask(tiFunc TurnoutInfoFunc) error { slice := turnoutInfoFunc() for _, turnoutInfo := range slice { turnoutInfo.lifeSignal = turnoutLifeSignal - err := sendTurnoutInfo(turnoutInfo) + // sendTurnoutInfo 发送道岔信息 + err := sendDynamicsMsg(encoderDynamicsTurnout(turnoutInfo)) if err != nil { zap.S().Error(err) } @@ -219,3 +188,45 @@ func runSendTurnoutStateTask(tiFunc TurnoutInfoFunc) error { }() return nil } + +// 解析动力学的列车信息 +func decoderDynamicsTrainInfo(buf []byte) *TrainInfo { + trainInfo := &TrainInfo{} + trainInfo.LifeSignal = binary.BigEndian.Uint16(buf[0:2]) + trainInfo.Number = buf[2] + trainInfo.Len = binary.BigEndian.Uint16(buf[3:5]) + trainInfo.Link = buf[5] + trainInfo.LinkOffset = binary.BigEndian.Uint32(buf[6:10]) + trainInfo.Slope = binary.BigEndian.Uint16(buf[10:12]) + b := buf[12] + trainInfo.UpSlope = (b & (1 << 7)) != 0 + trainInfo.Up = (b & (1 << 6)) != 0 + trainInfo.TotalResistance = binary.BigEndian.Uint32(buf[14:18]) + trainInfo.AirResistance = binary.BigEndian.Uint32(buf[18:22]) + trainInfo.SlopeResistance = binary.BigEndian.Uint32(buf[22:26]) + trainInfo.CurveResistance = binary.BigEndian.Uint32(buf[26:30]) + trainInfo.Speed = math.Float32frombits(binary.BigEndian.Uint32(buf[30:34])) + trainInfo.HeadSpeed1 = math.Float32frombits(binary.BigEndian.Uint32(buf[34:38])) + trainInfo.HeadSpeed2 = math.Float32frombits(binary.BigEndian.Uint32(buf[38:42])) + trainInfo.TailSpeed1 = math.Float32frombits(binary.BigEndian.Uint32(buf[42:46])) + trainInfo.TailSpeed2 = math.Float32frombits(binary.BigEndian.Uint32(buf[46:50])) + trainInfo.HeadRadarSpeed = math.Float32frombits(binary.BigEndian.Uint32(buf[50:54])) + trainInfo.TailRadarSpeed = math.Float32frombits(binary.BigEndian.Uint32(buf[54:58])) + return trainInfo +} + +// 将道岔转为动力学的消息 +func encoderDynamicsTurnout(info *TurnoutInfo) []byte { + var data []byte + data = binary.BigEndian.AppendUint16(data, info.lifeSignal) + data = binary.BigEndian.AppendUint16(data, info.Code) + var b byte + if info.NPosition { + b |= 1 << 7 + } + if info.RPosition { + b |= 1 << 6 + } + data = append(data, b) + return data +} diff --git a/main.go b/main.go index 1c70fb3..a97fb06 100644 --- a/main.go +++ b/main.go @@ -10,6 +10,7 @@ import ( "joylink.club/bj-rtsts-server/docs" "joylink.club/bj-rtsts-server/dynamics" "joylink.club/bj-rtsts-server/middleware" + "joylink.club/bj-rtsts-server/vobc" ) // @title CBTC测试系统API @@ -38,6 +39,7 @@ func main() { engine.GET("/swagger/*any", ginSwagger.WrapHandler(swaggerFiles.Handler)) go dynamics.RunUdpServer() + go vobc.RunUdpServer() serverConfig := config.Config.Server if serverConfig.Port == 0 { diff --git a/vobc/udp.go b/vobc/udp.go new file mode 100644 index 0000000..351d96f --- /dev/null +++ b/vobc/udp.go @@ -0,0 +1,200 @@ +package vobc + +import ( + "container/list" + "encoding/binary" + "fmt" + "math" + "net" + "sync" + + "github.com/panjf2000/gnet/v2" + "go.uber.org/zap" + "joylink.club/bj-rtsts-server/config" +) + +var ( + running bool + mutex sync.Mutex + receiveTrainLifeSignal uint16 //接收列车消息生命信号 + sendTrainLifeSignal uint16 //发送的列车消息生命信号 + trainLifeSignalInit bool //列车生命信号是否初始化 + limit uint16 = 10000 //用于处理生命信号循环使用产生的各种特殊处理 + trainInfoChan chan *ReceiveTrainInfo = make(chan *ReceiveTrainInfo) //列车消息队列 + //处理方法列表 + handlerList list.List +) + +func init() { + go func() { + for { + info := <-trainInfoChan + for e := handlerList.Front(); e != nil; e = e.Next() { + func() { + defer func() { + r := recover() + if r != nil { + zap.S().Errorf("列车信息处理函数报错") + } + }() + handler := e.Value.(TrainInfoHandler) + handler(info) + }() + } + } + }() +} + +type TrainInfoHandler func(info *ReceiveTrainInfo) + +type udpServer struct { + gnet.BuiltinEventEngine + + eng gnet.Engine + addr string + multicore bool +} + +// udp 启动时运行 +func (server *udpServer) OnBoot(eng gnet.Engine) gnet.Action { + server.eng = eng + zap.S().Infof("vobc udp server with multi-core=%t is listening on %s", server.multicore, server.addr) + return gnet.None +} + +// OnTraffic 接收到数据后的解析 +func (server *udpServer) OnTraffic(c gnet.Conn) gnet.Action { + defer func() { + if r := recover(); r != nil { + zap.L().Error("vobc udp服务数据解析异常", zap.Any("panic", r)) + } + }() + buf, _ := c.Next(-1) + lifeSignal := binary.BigEndian.Uint16(buf[0:2]) + if !trainLifeSignalInit { + trainLifeSignalInit = true + } else if receiveTrainLifeSignal < limit { + if lifeSignal < receiveTrainLifeSignal || lifeSignal > receiveTrainLifeSignal-limit { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, receiveTrainLifeSignal) + return gnet.None + } + } else if receiveTrainLifeSignal < math.MaxUint16-10000 { + if lifeSignal < receiveTrainLifeSignal { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, receiveTrainLifeSignal) + return gnet.None + } + } else { + if lifeSignal < receiveTrainLifeSignal && lifeSignal > receiveTrainLifeSignal+10000 { + zap.S().Debugf("丢弃列车信息[%d-%d]", lifeSignal, receiveTrainLifeSignal) + return gnet.None + } + } + receiveTrainLifeSignal = lifeSignal + trainInfo := decoderVobcTrainInfo(buf) + //列车消息队列 + trainInfoChan <- trainInfo + return gnet.None +} + +// 注册处理vobc处理方法 +func RegisterTrainInfoHandler(handler TrainInfoHandler) { + handlerList.PushBack(handler) +} + +// 创建UDP服务 +func RunUdpServer() { + server := &udpServer{addr: fmt.Sprintf("udp://:%d", config.Config.Vobc.LocalPort), multicore: false} + err := gnet.Run(server, server.addr, gnet.WithMulticore(server.multicore)) + zap.L().Fatal("vobc udp服务启动失败", zap.Error(err)) +} + +// 发送列车速度到VOBC +func SendTrainSpeedTask(speed float32) error { + if running { + return nil + } + mutex.Lock() + defer mutex.Unlock() + trainInfo := &SendTrainInfo{ + LifeSignal: sendTrainLifeSignal, + Speed: uint16(speed), + } + err := sendDynamicsMsg(encoderVobcTrainInfo(trainInfo)) + if err != nil { + zap.S().Error(err) + } + sendTrainLifeSignal++ + return err +} + +// UDP停止 +func Stop() { + mutex.Lock() + defer mutex.Unlock() + running = false +} + +// Vobc 消息发送消息 +func sendDynamicsMsg(buf []byte) error { + defer func() { + if r := recover(); r != nil { + zap.S().Error("发送列车速度信息失败", r) + } + }() + addr := fmt.Sprintf("%v:%v", config.Config.Vobc.Ip, config.Config.Vobc.RemotePort) + remoteAddr, _ := net.ResolveUDPAddr("udp", addr) + conn, err := net.DialUDP("udp", nil, remoteAddr) + if err != nil { + zap.S().Error("UDP通信失败", err) + return err + } + defer func(conn *net.UDPConn) { + err := conn.Close() + if err != nil { + zap.S().Error(err) + } + }(conn) + _, err = conn.Write(buf) + return err +} + +// 解析VOBC列车信息 +func decoderVobcTrainInfo(buf []byte) *ReceiveTrainInfo { + trainInfo := &ReceiveTrainInfo{} + trainInfo.LifeSignal = binary.BigEndian.Uint16(buf[0:2]) + b2 := buf[2] + trainInfo.Tc1Active = (b2 & (1 << 7)) != 0 + trainInfo.Tc2Active = (b2 & (1 << 6)) != 0 + trainInfo.DirectionForward = (b2 & (1 << 5)) != 0 + trainInfo.DirectionBackward = (b2 & (1 << 4)) != 0 + trainInfo.TractionStatus = (b2 & (1 << 3)) != 0 + trainInfo.BrakingStatus = (b2 & (1 << 2)) != 0 + trainInfo.EmergencyBrakingStatus = (b2 & (1 << 1)) != 0 + trainInfo.TurnbackStatus = (b2 & 1) != 0 + b3 := buf[3] + trainInfo.JumpStatus = (b3 & (1 << 7)) != 0 + trainInfo.ATO = (b3 & (1 << 6)) != 0 + trainInfo.FAM = (b3 & (1 << 5)) != 0 + trainInfo.CAM = (b3 & (1 << 4)) != 0 + trainInfo.TractionSafetyCircuit = (b3 & (1 << 3)) != 0 + trainInfo.ParkingBrakeStatus = (b3 & (1 << 2)) != 0 + trainInfo.MaintainBrakeStatus = (b3 & (1 << 1)) != 0 + trainInfo.TractionForce = binary.BigEndian.Uint16(buf[4:6]) + trainInfo.BrakeForce = binary.BigEndian.Uint16(buf[6:8]) + trainInfo.TrainLoad = binary.BigEndian.Uint16(buf[8:10]) + b4 := buf[15] + trainInfo.LeftDoorOpenCommand = (b4 & (1 << 7)) != 0 + trainInfo.RightDoorOpenCommand = (b4 & (1 << 6)) != 0 + trainInfo.LeftDoorCloseCommand = (b4 & (1 << 5)) != 0 + trainInfo.RightDoorCloseCommand = (b4 & (1 << 4)) != 0 + trainInfo.AllDoorClose = (b4 & (1 << 3)) != 0 + return trainInfo +} + +// 将道岔转为动力学的消息 +func encoderVobcTrainInfo(info *SendTrainInfo) []byte { + var data []byte + data = binary.BigEndian.AppendUint16(data, info.LifeSignal) + data = binary.BigEndian.AppendUint16(data, info.Speed) + return data +} diff --git a/vobc/udpData.go b/vobc/udpData.go new file mode 100644 index 0000000..88bc4ae --- /dev/null +++ b/vobc/udpData.go @@ -0,0 +1,65 @@ +package vobc + +// 接收到的列车信息 +type ReceiveTrainInfo struct { + //【0 1】两个字节 + // 生命信号 每个周期+1 + LifeSignal uint16 + //【2】 一个字节 + // TC1激活状态 1=激活 + Tc1Active bool + // TC2激活状态 1=激活 + Tc2Active bool + // 列车方向向前 1=方向向前 + DirectionForward bool + // 列车方向向后 1=方向向后 + DirectionBackward bool + // 列车牵引状态 1=牵引 + TractionStatus bool + // 列车制动状态 1=制动 + BrakingStatus bool + // 列车紧急制动状态 1=紧急制动 + EmergencyBrakingStatus bool + // 列车折返状态(AR) 1=折返 + TurnbackStatus bool + //【3】 一个字节 + // 跳跃状态 1=跳跃 + JumpStatus bool + // ATO模式 1=ATO模式 + ATO bool + // FAM模式 1=FAM模式 + FAM bool + // CAM模式 1=CAM模式 + CAM bool + // 牵引安全回路 1=牵引安全切除 + TractionSafetyCircuit bool + // 停放制动状态 1=停放施加 + ParkingBrakeStatus bool + // 保持制动状态 1=保持制动施加 + MaintainBrakeStatus bool + //【4 5】 两个字节 列车牵引力 100=1KN + TractionForce uint16 + //【6 7】 列车制动力 100=1KN + BrakeForce uint16 + //【8 9】 列车载荷 100=1ton + TrainLoad uint16 + // 【15】 一个字节 + // 列车开左门指令 1=开门 + LeftDoorOpenCommand bool + // 列车开右门指令 1=开门 + RightDoorOpenCommand bool + // 列车关左门指令 1=关门 + LeftDoorCloseCommand bool + // 列车关右门指令 1=关门 + RightDoorCloseCommand bool + // 整列车门关好 1=门关好 + AllDoorClose bool +} + +// 发送列车信息 +type SendTrainInfo struct { + // 生命信号 每个周期+1 + LifeSignal uint16 + // 列车速度 10=1km/h + Speed uint16 +}