From 5735f54b30e1281e782038cf88c89f0f1a64d13f Mon Sep 17 00:00:00 2001 From: thesai <1021828630@qq.com> Date: Fri, 15 Nov 2024 09:52:20 +0800 Subject: [PATCH] =?UTF-8?q?[=E9=87=8D=E5=86=99]11=E5=8F=B7=E7=BA=BFBTM?= =?UTF-8?q?=E4=B8=8EVOBC=E9=80=9A=E4=BF=A1=E6=9C=8D=E5=8A=A1?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/config.go | 2 +- rts-sim-module | 2 +- third_party/btm_vobc/beijing11/msg.go | 301 ++++++++++++++++++ third_party/btm_vobc/beijing11/service.go | 214 +++++++++++++ third_party/train_pc_sim/beijing11/service.go | 4 +- ts/test_simulation_manage.go | 5 +- 6 files changed, 522 insertions(+), 6 deletions(-) create mode 100644 third_party/btm_vobc/beijing11/msg.go create mode 100644 third_party/btm_vobc/beijing11/service.go diff --git a/config/config.go b/config/config.go index 631c44f..493d2b5 100644 --- a/config/config.go +++ b/config/config.go @@ -67,7 +67,7 @@ type ThirdPartyConfig struct { RsspAxleConfig RsspAxleConfig `json:"rsspAxleCfgs" description:"计轴通信配置"` //ElectricMachinery ElectricMachineryConfig `json:"electricMachinery" description:"电机配置"` ElectricMachinerys []ElectricMachineryConfig `json:"electricMachinerys" description:"电机配置"` - BtmCanet BtmCanetConfig `json:"btmCanet" description:"BTM关联的网关设备CANET配置"` + BtmCanet BtmCanetConfig `json:"btmCanet" description:"11号线工装配置"` //废字段再利用 CidcModbus []CidcModbusConfig `json:"cidcModbus" description:"联锁驱采Modbus接口配置"` Radar RadarConfig `json:"radar" description:"车载雷达相关配置"` Acc AccConfig `json:"acc" description:"车载加速计"` diff --git a/rts-sim-module b/rts-sim-module index 073f5e3..b7bc9bd 160000 --- a/rts-sim-module +++ b/rts-sim-module @@ -1 +1 @@ -Subproject commit 073f5e3156b66343ff283ba3530b4d3d63ca4461 +Subproject commit b7bc9bdb1f72aded0cd0d1f5c2d6970e6d1cef76 diff --git a/third_party/btm_vobc/beijing11/msg.go b/third_party/btm_vobc/beijing11/msg.go new file mode 100644 index 0000000..1629664 --- /dev/null +++ b/third_party/btm_vobc/beijing11/msg.go @@ -0,0 +1,301 @@ +package beijing11 + +import ( + "encoding/binary" + "github.com/snksoft/crc" + "joylink.club/bj-rtsts-server/util/myreader" +) + +const ( //数据帧外层增加用于区分ID命令帧和请求帧的帧类型字段 + id_Tupe = 0xF8 + rqst_Type = 0xE6 +) + +type frameType = byte + +const ( + frameType_ID frameType = 0x90 //ID命令帧 + frameType_Req frameType = 0x91 //请求帧 + frameType_Telegram frameType = 0x92 //报文帧 + frameType_Free frameType = 0x94 //空闲帧 +) + +type frameState = byte //帧正确/不正确（不好起名字，就这么着吧） +const ( //以下以外的值无效 + fs_Correct frameState = 0x06 //帧正确 + fs_Error frameState = 0x15 //不正确 + fs_Init frameState = 0x00 //开机 +) + +type telegramState = byte //空闲/报文数据 +const ( //以下以外的值无效 + telegram_free = 0x05 //空闲 + telegram_on = 0x0A //报文数据 + telegram_init = 0x00 //开机 +) + +type btmWorkState byte + +const ( + bws_normal btmWorkState = 0x00 //正常工作 + bws_minorFault btmWorkState = 0x04 //警告BTM有轻微故障（单套故障，整机能工作），向 BDMS 汇报； + bws_fault btmWorkState = 0xFF //BTM故障，不能正常工作；在MMI进行故障提示并向 ATS 汇报。 +) + +type ivState uint16 //电流/电压的状态 +const ( + iv_under ivState = 1 //欠流/欠压状态 + iv_over ivState = 2 //过流/过压状态 + iv_normal ivState = 3 //正常 +) + +type cableState uint16 //线缆状态 +const ( + cable_open cableState = 1 //开路 + cable_short cableState = 2 //短路 + cable_normal cableState = 3 //正常 +) + +type receiverBoardState uint16 + +const ( + rbs_doubleFault receiverBoardState = 0 //双通道故障 + rbs_singleFault receiverBoardState = 1 //单通道故障 + rbs_normal receiverBoardState = 3 //正常 +) + +type powerBoardState uint16 + +const ( + pbs_fault powerBoardState = 0 //故障 + pbs_singleFault powerBoardState = 1 //单通道故障 + pbs_doubleFault powerBoardState = 2 //双通道故障（文档写的单通道，应该是写错了） + pbs_normal powerBoardState = 3 //正常 +) + +type cpuWorkTemperatureState uint16 + +const ( + wts_tooHigh cpuWorkTemperatureState = 0 //过高 + wts_alarm cpuWorkTemperatureState = 1 //温度报警 + wts_unknown cpuWorkTemperatureState = 2 //未知 + wts_normal cpuWorkTemperatureState = 3 //正常 +) + +type baseFrame struct { + frameType frameType //帧类型 + len byte //帧长 + id byte //递增的ID码 +} + +func (b *baseFrame) decode(data []byte) error { + b.frameType = data[0] + b.len = data[1] + b.id = data[2] + return nil +} + +type idFrame struct { + baseFrame + btmId uint16 //BTM的ID + vobcId uint16 //VOBC的ID + vobcCycle uint32 //VOBC周期号（1~0xFFFFFFFF，0不使用） + reservedBytes []byte //预留字节（4字节） +} + +func (i *idFrame) decode(data []byte) error { + err := i.baseFrame.decode(data) + if err != nil { + return err + } + reader := myreader.NewReader(data[3:]) + i.btmId = binary.BigEndian.Uint16(reader.ReadBytes(2)) + i.vobcId = binary.BigEndian.Uint16(reader.ReadBytes(2)) + i.vobcCycle = binary.BigEndian.Uint32(reader.ReadBytes(4)) + return reader.Err +} + +type reqFrame struct { + baseFrame + frameState frameState //帧正确/不正确 + telegramState telegramState //空闲/报文数据 + sn byte //报文序列号。1-255；开机时使用 0 + reservedBytes []byte //预留字节（10字节） + vobcCycle uint32 //VOBC周期号（1~0xFFFFFFFF，0不使用） + time []byte //年月日时分秒各占一个字节 + speed uint16 //速度 单位：cm/s + vobcCycleDistance uint16 //周期走行距离 单位：cm +} + +func (r *reqFrame) decode(data []byte) error { + err := r.baseFrame.decode(data) + if err != nil { + return err + } + reader := myreader.NewReader(data[3:]) + r.frameState = reader.ReadByte() + r.telegramState = reader.ReadByte() + r.sn = reader.ReadByte() + r.reservedBytes = reader.ReadBytes(10) + r.vobcCycle = binary.BigEndian.Uint32(reader.ReadBytes(4)) + r.time = reader.ReadBytes(6) + r.speed = binary.BigEndian.Uint16(reader.ReadBytes(2)) + r.vobcCycleDistance = binary.BigEndian.Uint16(reader.ReadBytes(2)) + return reader.Err +} + +type telegramFrame struct { + id byte //递增的id码 + headTTLTime uint16 //前沿TTL时间。单位为ms，溢出为0xffff，高字节在前；车体及应答器地面环境理想情况下误差小于5ms + sn byte //报文序列号。1-255，不使用0 + btmWorkState btmWorkState //BTM工作状态 + channel1GoodBitRate byte //1通道好码率（0~100） + channel2GoodBitRate byte //2通道好码率（0~100） + decodeDuration uint16 //解码时间。从包络前沿到解码成功的时间（没写单位，猜测是ms） + //reservedBytes []byte //预留（9字节） + tailTTLTime uint16 //后沿TTL时间。单位为ms，溢出为0xffff，高字节在前；车体及应答器地面环境理想情况下误差小于5ms + telegram []byte //应答器报文（104字节） + responseDuration byte //响应时间。0~150，其他非法，单位0.1ms，误差小于3ms + vobcReqCycle uint32 //VOBC请求帧周期号。（1~0xFFFFFFFF，不使用0） +} + +func (t *telegramFrame) encode() []byte { + data := make([]byte, 0, 150) + data = append(data, frameType_Telegram) + data = append(data, 0) //帧长度，占位 + data = append(data, t.id) + data = binary.BigEndian.AppendUint16(data, t.headTTLTime) + data = append(data, t.sn) + data = append(data, byte(t.btmWorkState)) + data = append(data, t.channel1GoodBitRate) + data = append(data, t.channel2GoodBitRate) + data = binary.BigEndian.AppendUint16(data, t.decodeDuration) + data = append(data, make([]byte, 9)...) + data = binary.BigEndian.AppendUint16(data, t.tailTTLTime) + data = append(data, t.telegram...) + data = append(data, t.responseDuration) + data = binary.BigEndian.AppendUint32(data, t.vobcReqCycle) + data[1] = byte(len(data) + 4) //帧长度赋值 + data = binary.BigEndian.AppendUint32(data, crc32(data)) + return data +} + +type freeFrame struct { + id byte //递增的id码 + sn byte //报文序列号。1-255，开机时为0 + btmWorkState btmWorkState //BTM工作状态 + workTemperature byte //工作温度。单位℃ + //reservedBytes []byte //预留（6字节） + //14-15字节。功放板、天线状态 + amp1CurrentState ivState //功放1电流状态（0-1位） + amp1VoltageState ivState //功放1电压状态（2-3位） + amp2CurrentState ivState //功放2电流状态（4-5位） + amp2VoltageState ivState //功放2电压状态（6-7位） + antenna1Fault bool //天线1状态（8-9位）（1故障，3正常） + cable1State cableState //线缆1状态（10-11位） + antenna2Fault bool //天线1状态（12-13位）（1故障，3正常） + cable2State cableState //线缆2状态（14-15位） + //16-17字节。接收板状态 + selfCheckChannel1Fault bool //上行自检码检测通道1（0位）（1正常，0故障） + selfCheckChannel2Fault bool //上行自检码检测通道2（1位）（1正常，0故障） + fskChannel1Fault bool //FSK连接线状态通道1（2位）（1正常，0故障） + fskChannel2Fault bool //FSK连接线状态通道2（3位）（1正常，0故障） + receiverBoardState receiverBoardState //接收板状态（6-7位） + //18-19字节。电源板状态 + channel1_24v ivState //通道1 24V状态（0-1位） + channel2_24v ivState //通道2 24V状态（2-3位） + channel1_23vFault bool //通道1 23V状态（4位）（1正常，0故障） + channel2_23vFault bool //通道2 23V状态（5位）（1正常，0故障） + powerBoardState powerBoardState //电源板状态（14-15位） + //20-21字节。处理器板 + cpuBoardId byte //板卡ID槽位号（0-1位）（0-1号板卡，1-2号板卡，类推） + cpuWorkTemperatureState cpuWorkTemperatureState //工作温度状态（2-3位） + + //freeData []byte //空闲数据（104字节，全填0） + responseDuration byte //相应时间，0-150，其他非法，单位0.1ms + vobcCycle uint32 //VOBC请求帧周期号 +} + +func (f *freeFrame) encode() []byte { + data := make([]byte, 0, 150) + data = append(data, frameType_Free) + data = append(data, 0) //帧长度，占位 + data = append(data, f.id) + data = append(data, f.sn) + data = append(data, byte(f.btmWorkState)) + data = append(data, f.workTemperature) + data = append(data, make([]byte, 6)...) + //功放板、天线状态 + var ampState uint16 + ampState += uint16(f.amp1CurrentState) << 15 + ampState += uint16(f.amp1VoltageState) << 13 + ampState += uint16(f.amp2CurrentState) << 11 + ampState += uint16(f.amp2VoltageState) << 9 + if f.antenna1Fault { + ampState += uint16(1) << 7 + } else { + ampState += uint16(3) << 7 + } + ampState += uint16(f.cable1State) << 5 + if f.antenna2Fault { + ampState += uint16(1) << 3 + } else { + ampState += uint16(3) << 3 + } + ampState += uint16(f.cable2State) + data = binary.BigEndian.AppendUint16(data, ampState) + //接收板状态 + var receiverState uint16 + if !f.selfCheckChannel1Fault { + receiverState += uint16(1) << 15 + } + if !f.selfCheckChannel2Fault { + receiverState += uint16(1) << 14 + } + if !f.fskChannel1Fault { + receiverState += uint16(1) << 13 + } + if !f.fskChannel2Fault { + receiverState += uint16(1) << 12 + } + receiverState += uint16(f.receiverBoardState) << 9 + data = binary.BigEndian.AppendUint16(data, receiverState) + //电源板状态 + var powerState uint16 + powerState += uint16(f.channel1_24v) << 15 + powerState += uint16(f.channel2_24v) << 13 + if !f.channel1_23vFault { + powerState += uint16(1) << 11 + } + if !f.channel2_23vFault { + powerState += uint16(1) << 10 + } + powerState += uint16(f.powerBoardState) + data = binary.BigEndian.AppendUint16(data, powerState) + //处理器板 + var cpuState uint16 + cpuState += uint16(f.cpuBoardId) << 15 + cpuState += uint16(f.cpuWorkTemperatureState) << 13 + data = binary.BigEndian.AppendUint16(data, cpuState) + //其它 + data = append(data, make([]byte, 104)...) + data = append(data, f.responseDuration) + data = binary.BigEndian.AppendUint32(data, f.vobcCycle) + + data[1] = byte(len(data) + 4) //帧长度赋值 + data = binary.BigEndian.AppendUint32(data, crc32(data)) + return data +} + +var crcHash = crc.NewHash(&crc.Parameters{ + Width: 32, + Polynomial: crc.CRC32.Polynomial, + ReflectIn: false, + ReflectOut: false, + Init: 0xFFFFFFFF, + FinalXor: 0, +}) + +func crc32(data []byte) uint32 { + return uint32(crcHash.CalculateCRC(data)) +} diff --git a/third_party/btm_vobc/beijing11/service.go b/third_party/btm_vobc/beijing11/service.go new file mode 100644 index 0000000..ad639c3 --- /dev/null +++ b/third_party/btm_vobc/beijing11/service.go @@ -0,0 +1,214 @@ +package beijing11 + +import ( + "bytes" + "encoding/hex" + "fmt" + "joylink.club/bj-rtsts-server/dto/state_proto" + "joylink.club/bj-rtsts-server/third_party/message" + "joylink.club/bj-rtsts-server/third_party/tpapi" + "joylink.club/bj-rtsts-server/third_party/udp" + "joylink.club/bj-rtsts-server/ts/simulation/wayside/memory" + "log/slog" + "sync" +) + +var ( + logTag = "[北京11号线BTM-VOBC通信]" + privateLogger *slog.Logger + loggerInit sync.Once +) + +var ( + mu = sync.Mutex{} //启动任务时使用，避免重复启动任务 + serviceCtx *serviceContext //当前正在运行的服务 +) + +type serviceContext struct { + simulation *memory.VerifySimulation + client udp.UdpClient + server udp.UdpServer + + id byte //无论何时传输数据，该数都在 1-255 范围内递增，在错误重传时也是递增的，255 之后是 1，不使用 0 + sn byte //报文序列号。1-255；开机时使用0 + + lastCmdId byte //vobc ID命令帧最新的ID码（1~255，不使用0） + lastReqId byte //vobc 请求帧最新的ID码（1~255，不使用0） + btmId uint16 //BTM的id + vobcId uint16 //VOBC的id + + state tpapi.ThirdPartyApiServiceState +} + +func Start(sim *memory.VerifySimulation) { + config := sim.GetRunConfig().BtmVobc + client := udp.NewClient(fmt.Sprintf("%s:%d", config.RemoteIp, config.RemoteUdpPort)) + serviceCtx := serviceContext{ + simulation: sim, + client: client, + } + server := udp.NewServer(fmt.Sprintf(":%d", config.LocalUdpPort), serviceCtx.handle) + err := server.Listen() + if err != nil { + Stop(sim) + return + } + serviceCtx.server = server + serviceCtx.state = tpapi.ThirdPartyState_Normal +} + +func Stop(sim *memory.VerifySimulation) { + if serviceCtx != nil && serviceCtx.simulation == sim { + if serviceCtx.server != nil { + serviceCtx.server.Close() + } + if serviceCtx.client != nil { + serviceCtx.client.Close() + } + serviceCtx = nil + logger().Info("服务停止") + } +} + +func (s *serviceContext) handle(data []byte) { + logger().Info(fmt.Sprintf("收到数据：%x", data)) + if !bytes.HasPrefix(data, []byte{0xFF, 0xFE}) || !bytes.HasSuffix(data, []byte{0xFF, 0xFD}) { + logger().Error("帧头/帧尾不对，丢弃数据") + return + } + decodeBytes, _ := message.TranslateFromFFFE(data[3 : len(data)-2]) + switch data[2] { + case id_Tupe: + frame := idFrame{} + err := frame.decode(decodeBytes) + if err != nil { + logger().Error(fmt.Sprintf("id命令帧解析出错：%s", err)) + return + } + s.handleIdFrame(&frame) + case rqst_Type: + frame := reqFrame{} + err := frame.decode(decodeBytes[20 : len(decodeBytes)-2]) //RSSP-I协议部分不做校验，直接忽略 + if err != nil { + logger().Error(fmt.Sprintf("请求帧解析出错：%s", err)) + return + } + s.handleReqFrame(&frame) + } +} + +func (s *serviceContext) handleIdFrame(frame *idFrame) { + if frame.id < s.lastCmdId { + logger().Error(fmt.Sprintf("vobc ID命令帧id倒退[%d：%d]", s.lastCmdId, frame.id)) + } + s.lastCmdId = frame.id + s.btmId = frame.btmId + s.vobcId = frame.vobcId + rspFrame := freeFrame{ + id: s.nextId(), + sn: s.nextSn(), + btmWorkState: bws_normal, + workTemperature: 20, + amp1CurrentState: iv_normal, + amp1VoltageState: iv_normal, + amp2CurrentState: iv_normal, + amp2VoltageState: iv_normal, + antenna1Fault: false, + cable1State: cable_normal, + antenna2Fault: false, + cable2State: cable_normal, + selfCheckChannel1Fault: false, + selfCheckChannel2Fault: false, + fskChannel1Fault: false, + fskChannel2Fault: false, + receiverBoardState: rbs_normal, + channel1_24v: iv_normal, + channel2_24v: iv_normal, + channel1_23vFault: false, + channel2_23vFault: false, + powerBoardState: pbs_normal, + cpuBoardId: 0, + cpuWorkTemperatureState: wts_normal, + responseDuration: 0, + vobcCycle: frame.vobcCycle, + } + encode := rspFrame.encode() + err := s.client.Send(encode) + if err != nil { + logger().Error("发送数据失败") + } else { + logger().Info(fmt.Sprintf("回复ID命令帧：%x", encode)) + } +} + +func (s *serviceContext) handleReqFrame(frame *reqFrame) { + if frame.frameState == fs_Error { + logger().Info("上一帧的报文序列或者CRC检查不正确") + } + if frame.id <= s.lastReqId { + logger().Info(fmt.Sprintf("vobc请求帧id倒退【%d：%d】", s.lastReqId, frame.id)) + return + } + s.lastReqId = frame.id + + s.simulation.Memory.Status.TrainStateMap.Range(func(_, value any) bool { + trainState := value.(*state_proto.TrainState) + if trainState.ConnState.Conn && trainState.ConnState.ConnType == state_proto.TrainConnState_PC_SIM { + telFrame := telegramFrame{} + telFrame.id = s.nextId() + telFrame.headTTLTime = 1 + telFrame.sn = s.nextSn() + telFrame.btmWorkState = bws_normal + telFrame.channel1GoodBitRate = 100 + telFrame.channel2GoodBitRate = 100 + telFrame.decodeDuration = 1 + telFrame.tailTTLTime = 1 + for _, bs := range trainState.BtmBaliseCacheA.BaliseList { + if !bs.IsSend { + telegram, err := hex.DecodeString(bs.Telegram) + if err != nil { + logger().Error(fmt.Sprintf("用户报文解码出错：%s", err)) + continue + } + telFrame.telegram = telegram + } + } + telFrame.responseDuration = 10 + telFrame.vobcReqCycle = frame.vobcCycle + + encode := telFrame.encode() + err := s.client.Send(encode) + if err != nil { + logger().Error("发送数据失败") + } else { + logger().Info(fmt.Sprintf("回复vobc请求帧：%x", encode)) + } + return false + } + return true + }) +} + +func (s *serviceContext) nextId() byte { + s.id++ + if s.id == 0 { + s.id++ + } + return s.id +} + +func (s *serviceContext) nextSn() byte { + tmp := s.sn + s.sn++ + if s.sn == 0 { + s.sn++ + } + return tmp +} + +func logger() *slog.Logger { + loggerInit.Do(func() { + privateLogger = slog.Default().With("tag", logTag) + }) + return privateLogger +} diff --git a/third_party/train_pc_sim/beijing11/service.go b/third_party/train_pc_sim/beijing11/service.go index d31571f..a4dc922 100644 --- a/third_party/train_pc_sim/beijing11/service.go +++ b/third_party/train_pc_sim/beijing11/service.go @@ -61,7 +61,7 @@ func (s *serviceContext) ServiceDesc() string { } func Start(simulation *memory.VerifySimulation) { - config := simulation.GetRunConfig().BtmVobc + config := simulation.GetRunConfig().BtmCanet if !config.Open { return } @@ -82,7 +82,7 @@ func Start(simulation *memory.VerifySimulation) { } func Stop(simulation *memory.VerifySimulation) { - if serviceCtx.simulation == simulation { + if serviceCtx != nil && serviceCtx.simulation == simulation { serviceCtx.cancelFunc() if serviceCtx.client != nil { serviceCtx.client.Close() diff --git a/ts/test_simulation_manage.go b/ts/test_simulation_manage.go index 71a0819..fbbd24b 100644 --- a/ts/test_simulation_manage.go +++ b/ts/test_simulation_manage.go @@ -5,6 +5,7 @@ import ( "joylink.club/bj-rtsts-server/service" "joylink.club/bj-rtsts-server/third_party/acc" axleBeijing12 "joylink.club/bj-rtsts-server/third_party/axle_device/beijing12" + btmBeijing11 "joylink.club/bj-rtsts-server/third_party/btm_vobc/beijing11" "joylink.club/bj-rtsts-server/third_party/interlock/beijing11" "joylink.club/bj-rtsts-server/third_party/interlock/beijing12" "joylink.club/bj-rtsts-server/third_party/radar" @@ -171,8 +172,8 @@ func runThirdParty(s *memory.VerifySimulation) error { //列车加速计发送vobc acc.Default().Start(s) train_pc_sim.Default().Start(s) - //btm vobc - semi_physical_train.BtmDefault().Start(s) + //11号线VOBC通信 + btmBeijing11.Start(s) //11号线工装通信 trainBeijing11.Start(s) return nil