package train_pc_sim import ( "context" "encoding/hex" "fmt" "joylink.club/bj-rtsts-server/config" "joylink.club/bj-rtsts-server/dto/state_proto" "joylink.club/bj-rtsts-server/sys_error" "joylink.club/bj-rtsts-server/third_party/message" "joylink.club/bj-rtsts-server/third_party/tcp" "joylink.club/bj-rtsts-server/third_party/tpapi" "log/slog" "math" "sync" "time" ) //type TrainControlEvent struct { // Type byte // Data []byte //} //var FireTrainControlEventType = ecs.NewEventType[TrainControlEvent]() type TrainPcSim interface { tpapi.ThirdPartyApiService Start(pcSimManage TrainPcSimManage) Stop() // SendDriverActive Deprecated 发送驾驶端激活 //SendDriverActive(train *state_proto.TrainState) // SendHandleSwitch 发送牵引制动手柄 SendHandleSwitch(oldTraction, oldBrakeForce int64, tractionState bool, train *state_proto.TrainState) // SendTrainDirection 列车运行方向 //因文档说明不清楚,在调用的时候目前是注释状态,现场调试可能会用到 SendTrainDirection(train *state_proto.TrainState, trainForward, trainBackward bool) //发送应答器信息数据 SendBaliseData2(train *state_proto.TrainState, trainClientPort state_proto.TrainState_TrainPort, msgType byte, data []byte) //发布列车控制的相关事件 //PublishTrainControlEvent(train *state_proto.TrainState, events []TrainControlEvent) SendTrainControlMsg2(train *state_proto.TrainState, baseMessage []message.TrainPcSimBaseMessage, trainClientPort state_proto.TrainState_TrainPort) // CreateOrRemoveSpeedPLace 创建或删除速度位置信息 //CreateOrRemoveSpeedPLace(train *state_proto.TrainState) // CreateOrRemoveTrain 创建或删除列车 CreateOrRemoveTrain(train *state_proto.TrainState, isCreate bool) error // TrainPluseCount 计算列车脉冲 TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32) } type TrainPcSimManage interface { GetTrainPcSimConfig() []config.VehiclePCSimConfig //GetConnTrain() *state_proto.TrainState GetConnTrain2() []*state_proto.TrainState //获取列车模拟量数据 ObtainTrainDigitalMockData(train *state_proto.TrainState) []message.TrainPcSimBaseMessage // TrainPcSimDigitalOutInfoHandle 4.4.1. 车载输出数字量信息报文内容 TrainPcSimDigitalOutInfoHandle(pc *TrainPcReciverData, train *state_proto.TrainState, data []byte) bool // TrainPcSimDigitalReportHandle 4.4.2. 车载输出数字反馈量信息报文内容 TrainPcSimDigitalReportHandle(train *state_proto.TrainState, data []byte) FindConnTrain(ct state_proto.TrainConnState_TrainConnType) *state_proto.TrainState // TrainPcSimMockInfo 门模式 //TrainDoorModeHandle(state byte) //处理列车pc仿真模拟量数据 TrainPcSimMockInfo(train *state_proto.TrainState, data []byte) // TrainBtmQuery 处理列车btm查询 TrainBtmQuery2(train *state_proto.TrainState, data []byte, trainClientPort state_proto.TrainState_TrainPort) } type trainPcSimService struct { state tpapi.ThirdPartyApiServiceState //newPcSimclientMap map[string]*TrainPcReciverData newPcSimclientMap3 map[string][]*TrainPcReciverData cancleContextFun context.CancelFunc context context.Context trainPcSimManage TrainPcSimManage configs []config.VehiclePCSimConfig } var ( initLock = &sync.Mutex{} singleObj *trainPcSimService ) func Default() TrainPcSim { defer initLock.Unlock() initLock.Lock() if singleObj == nil { singleObj = &trainPcSimService{} } return singleObj } const Name = "车载pc仿真" func (d *trainPcSimService) Name() string { return "" } func (d *trainPcSimService) State() tpapi.ThirdPartyApiServiceState { return tpapi.ThirdPartyState_Closed } func (d *trainPcSimService) FindAppendApiService() []tpapi.ThirdPartyApiService { return d.findAllThirdPartState() } func (d *trainPcSimService) TrueService() bool { return false } func (d *trainPcSimService) ServiceDesc() string { return Name } func FindTrainPcSimClientKey2(t *state_proto.TrainState) string { return t.ConnState.TypeName } func (d *trainPcSimService) findTrainConnForPort2(sta *state_proto.TrainState, trainClientPort state_proto.TrainState_TrainPort) (*TrainPcReciverData, error) { rds := d.newPcSimclientMap3[sta.ConnState.TypeName] if rds == nil { return nil, fmt.Errorf("") } for _, rd := range rds { if rd.RealTrainPort == trainClientPort { return rd, nil } } return nil, fmt.Errorf("") } func (d *trainPcSimService) findTrainConn(sta *state_proto.TrainState) (*TrainPcReciverData, error) { rds := d.newPcSimclientMap3[sta.ConnState.TypeName] if rds == nil { return nil, fmt.Errorf("") } if sta.VobcState.Tc1Active { return rds[0], nil } else { return rds[1], nil } } func (d *trainPcSimService) findTrainAllConn(sta *state_proto.TrainState) []*TrainPcReciverData { rds := d.newPcSimclientMap3[sta.ConnState.TypeName] return rds } func (d *trainPcSimService) findAllThirdPartState() []tpapi.ThirdPartyApiService { services := make([]tpapi.ThirdPartyApiService, 0) for _, data := range d.newPcSimclientMap3 { for _, rd := range data { services = append(services, rd) } } return services } // 速度(单位mm/s)对应的脉冲数:速度*200/pi/840; // 里程(单位mm)对应的脉冲:总里程*200/pi/840 func pluseCountSpeed(wheelDiameter int32, speedMeter float32) uint32 { pluseCountData := speedMeter * 200 / math.Pi / float32(wheelDiameter) return uint32(pluseCountData) } func (d *trainPcSimService) pluseSpeed(pc *state_proto.SensorSpeedPulseCount, wheelDiameter int32) (uint32, float32) { defer initLock.Unlock() initLock.Lock() var sum float32 = 0 pcLen := len(pc.PulseCount3) if pcLen == 0 { return 0, 0 } for _, f := range pc.PulseCount3 { sum += f } d.trainPluseCountReset(pc) speed := sum / float32(pcLen) return pluseCountSpeed(wheelDiameter, speed*1000), speed } func (d *trainPcSimService) TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32) { defer initLock.Unlock() initLock.Lock() select { case <-d.context.Done(): return default: } for _, sd := range d.findTrainAllConn(sta) { if sd.speedPlace != nil { sd.speedPlace.PulseCount1 += sta.DynamicState.Displacement } } for _, pc := range sta.PulseCountMap { if sta.TrainRunUp { if sta.TrainEndsA.SpeedSensorEnableA || sta.TrainEndsA.SpeedSensorEnableB { pc.PulseCount1 = pluseCountSpeed(sta.WheelDiameter, h1) pc.PulseCount3 = append(pc.PulseCount3, h1) } } else { if sta.TrainEndsB.SpeedSensorEnableA || sta.TrainEndsB.SpeedSensorEnableB { pc.PulseCount1 = pluseCountSpeed(sta.WheelDiameter, t1) pc.PulseCount3 = append(pc.PulseCount3, t1) } } } } func (d *trainPcSimService) trainPluseCountReset(pc *state_proto.SensorSpeedPulseCount) { pc.PulseCount1 = 0 pc.PulseCount3 = make([]float32, 0) } func (d *trainPcSimService) newCloseAllConn() { trains := d.trainPcSimManage.GetConnTrain2() for _, train := range trains { d.CreateOrRemoveTrain(train, false) } } func (d *trainPcSimService) newCloseConn(clientKey string) { rds := d.newPcSimclientMap3[clientKey] if rds != nil { for _, rd := range rds { rd.tcpClient.Close() rd.tcpClient = nil rd.train = nil rd.speedPlace = nil } } } func (d *trainPcSimService) findConfig(configName string) (*config.VehiclePCSimConfig, error) { for _, cfg := range d.configs { if cfg.Open && cfg.ConfigName == configName { return &cfg, nil } } return nil, fmt.Errorf("未找到对应的车载pc连接配置") } func (d *trainPcSimService) initConn2(clientKey string) error { rds := d.newPcSimclientMap3[clientKey] rd1 := rds[0] rd2 := rds[1] cfg, cfgErr := d.findConfig(clientKey) if cfgErr != nil { return sys_error.New(fmt.Sprintf("没找到对应的配置信息 key:%v", clientKey), cfgErr) } if !cfg.OpenB && !cfg.OpenA { return sys_error.New(fmt.Sprintf("配置:%v A,B端配置均为打开", clientKey)) } e1 := d.connServer(cfg.OpenA, cfg.APcSimIp, cfg.APcSimPort, rd1) if e1 != nil { rd1.updateState(tpapi.ThirdPartyState_Broken) return sys_error.New(fmt.Sprintf("配置:%v 端口A连接失败", clientKey)) } e2 := d.connServer(cfg.OpenB, cfg.BPcSimIp, cfg.BPcSimPort, rd2) if e2 != nil { rd1.updateState(tpapi.ThirdPartyState_Broken) return sys_error.New(fmt.Sprintf("配置:%v 端口B连接失败", clientKey)) } if rd1.success { //rd1.aPort = true rd1.RealTrainPort = state_proto.TrainState_PORT_A } if rd2.success { rd2.RealTrainPort = state_proto.TrainState_PORT_B } return nil } func (d *trainPcSimService) connServer(open bool, ip string, port uint32, rd *TrainPcReciverData) *sys_error.BusinessError { if rd != nil && rd.tcpClient != nil && rd.tcpClient.IsConning() { return nil } else { rd.tcpClient = nil } if !open { rd.success = false return nil } addr := fmt.Sprintf("%v:%v", ip, port) //slog.Info(addr, "连接.,...") client2, err := tcp.StartTcpClient(addr, rd.receiverDataHandle, rd.readError) if err != nil { return sys_error.New(fmt.Sprintf("车载atp连接失败,add:%v ,message:%v", addr, err)) } else { rd.success = true rd.tcpClient = client2 } return nil } func (d *trainPcSimService) Start(pcSimManage TrainPcSimManage) { configs := pcSimManage.GetTrainPcSimConfig() //d.newPcSimclientMap = make(map[string]*TrainPcReciverData) d.newPcSimclientMap3 = make(map[string][]*TrainPcReciverData) if len(configs) <= 0 { slog.Info("车载pc仿真配置未开启") return } closedCount := 0 for _, c := range configs { if !c.Open { closedCount++ } else { ck := c.ConfigName pcReceivers := make([]*TrainPcReciverData, 2) for i := 0; i < 2; i++ { ss := fmt.Sprintf("%v%v", c.ConfigName, i) pcReciver := &TrainPcReciverData{clientKey: ss, pcSimManage: pcSimManage} pcReciver.updateState(tpapi.ThirdPartyState_Closed) pcReceivers[i] = pcReciver } d.newPcSimclientMap3[ck] = pcReceivers //d.newPcSimclientMap[ck] = pcReciver } } if closedCount == len(configs) { slog.Error("车载pc仿真配置未开启") return } d.configs = configs ctx, ctxFun := context.WithCancel(context.Background()) d.cancleContextFun = ctxFun d.context = ctx d.trainPcSimManage = pcSimManage go d.sendTrainLocationAndSpeedTask(ctx) } func (d *trainPcSimService) Stop() { for _, rds := range d.newPcSimclientMap3 { for _, rd := range rds { rd.updateState(tpapi.ThirdPartyState_Closed) } } if d.cancleContextFun != nil { d.cancleContextFun() d.cancleContextFun = nil } d.newCloseAllConn() } func (d *trainPcSimService) CreateOrRemoveTrain(train *state_proto.TrainState, isCreate bool) error { clientKey := FindTrainPcSimClientKey2(train) data := []byte{message.FLAG_CAMMAND_REMOVE_TRAIN} if isCreate { err := d.initConn2(clientKey) if err != nil { d.newCloseConn(clientKey) return err } data[0] = message.FLAG_CAMMAND_CREATE_TRAIN } msg := &message.TrainPcSimBaseMessage{Data: data, Type: message.RECIVE_TRAIN_CREATE_REMOVE} rds := d.newPcSimclientMap3[clientKey] if rds != nil { for index, rd := range rds { if rd != nil && rd.success { slog.Info(fmt.Sprintf("index%v---rd client:%v clientnil :%v", index, rd.tcpClient, rd.tcpClient == nil)) initTrainErr := d.initTrain(rd, train, isCreate, msg) if !isCreate { d.newCloseConn(clientKey) } if initTrainErr != nil { return initTrainErr } } } } return nil } func (d *trainPcSimService) initTrain(rd *TrainPcReciverData, train *state_proto.TrainState, isCreate bool, trains *message.TrainPcSimBaseMessage) error { msgs := make([]message.TrainPcSimBaseMessage, 0) sendMsg := make([]byte, 0) rd.speedPlace = &message.TrainSpeedPlaceReportMsg{} train.PulseCountMap = make(map[int32]*state_proto.SensorSpeedPulseCount) train.PulseCountMap[int32(state_proto.TrainState_PORT_A.Number())] = &state_proto.SensorSpeedPulseCount{} train.PulseCountMap[int32(state_proto.TrainState_PORT_B.Number())] = &state_proto.SensorSpeedPulseCount{} rd.train = train tcc := train.Tcc rd.TrainConnInitComplate = false rd.LineInitTimeStamp = 0 if isCreate { tmpMsgs := d.trainPcSimManage.ObtainTrainDigitalMockData(train) msgs = append(msgs, tmpMsgs...) msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{0x00}, Type: message.RECIVE_TRAIN_DOOR_MODE}) //门模式 msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{}, Type: message.RECIVE_TRAIN_BTN_CLEAR_ALL_PRE_DATA}) //清空应答器 msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{}, Type: message.SENDER_TRAIN_TC_ACTIVE}) //清空应答器 } else { train.VobcState.Tc1Active = false train.VobcState.Tc2Active = false for _, key := range tcc.DriverKey { key.Val = false } msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{message.TRAIN_BRAKE_STATE, 0}, Type: message.SENDER_TRAIN_OUTR_INFO}) //驾驶室激活 msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{message.KEY_STATE, 0}, Type: message.SENDER_TRAIN_OUTR_INFO}) //驾驶室激活 msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{}, Type: message.SENDER_TRAIN_TC_NOT_ACTIVE}) //驾驶室激活 } for _, msg := range msgs { data := msg.Encode() sendMsg = append(sendMsg, data...) } sendMsg = append(sendMsg, trains.Encode()...) hexData := hex.EncodeToString(sendMsg) slog.Info(fmt.Sprintf("发送列车初始化消息:%v", hexData)) rd.tcpClient.Send(sendMsg) return nil } // 依据文档80ms发送列车速度位置 func (d *trainPcSimService) sendTrainLocationAndSpeedTask(ctx context.Context) { for range time.Tick(time.Millisecond * 80) { select { case <-ctx.Done(): return default: } trains := d.trainPcSimManage.GetConnTrain2() for _, train := range trains { if train.ConnState.Conn { for numKey, pc := range train.PulseCountMap { trainPort := state_proto.TrainState_TrainPort(numKey) trainClient, _ := d.findTrainConnForPort2(train, trainPort) if trainClient.success { if trainClient.speedPlace == nil || trainClient.tcpClient == nil { slog.Error(fmt.Sprintf("pc仿真速度位置脉冲对象为空 列车id:%v", train.Id)) continue } if trainClient.ConnError() { continue } s1, speed := d.pluseSpeed(pc, train.WheelDiameter) runDir := d.trainDirection(speed, train, trainClient.RealTrainPort) disPluse := pluseCountSpeed(train.WheelDiameter, trainClient.speedPlace.PulseCount1) data := trainClient.speedPlace.Encode(runDir, s1, disPluse) bm := &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_LOCATION_INFO, Data: data} dataCode := bm.Encode() slog.Info(fmt.Sprintf("发送列车速度位置,列车:%v,列车服务端:%v,列车速度:%v,计数脉冲: %v,累计里程: %v ,发送数据:%X", train.Id, trainClient.RealTrainPort.String(), speed, s1, trainClient.speedPlace.PulseCount1, dataCode)) err := trainClient.tcpClient.Send(dataCode) if err != nil { slog.Error(fmt.Sprintf("发送列车速度位置失败,列车:%v,发送数据:%v", train.Id, hex.EncodeToString(dataCode))) } } } } } } } func (d *trainPcSimService) trainDirection(speed float32, train *state_proto.TrainState, clientPort state_proto.TrainState_TrainPort) uint16 { runDir := uint16(2) vobc := train.VobcState if speed == 0 || train.TrainPort == state_proto.TrainState_PORT_NONE || (vobc.DirectionForward == false && vobc.DirectionBackward == false) { return runDir } if vobc.DirectionForward { runDir = 1 } else if vobc.DirectionBackward { runDir = 0 } if train.TrainPort != clientPort { if vobc.DirectionForward { runDir = 0 } else if vobc.DirectionBackward { runDir = 1 } } return runDir } func (d *trainPcSimService) SendHandleSwitch(oldTraction, oldBrakeForce int64, tractionState bool, train *state_proto.TrainState) { trainClient, trainDataErr := d.findTrainConn(train) if trainDataErr != nil { slog.Error(fmt.Sprintf("发送列车牵引失败,未找到对应的列车id:%v", train.Id)) return } tc := train.ConnState if tc.Conn { vobc := train.VobcState msg := &message.TrainPcSimBaseMessage{} newTraction := vobc.TractionForce if tractionState { if newTraction <= oldTraction && newTraction <= 0 { //手柄取消前进 msg.Type = message.RECIVE_TRAIN_HAND_KEY_CANCLE_FORWARD } else { //手柄前进 msg.Type = message.SENDER_TRAIN_HAND_KEY_FORWARD } } else { /*if newBrake >= newOldBrakeForce && newBrake == 0 { //手柄取消后退 msg.Type = message.RECIVE_TRAIN_HAND_KEY_CACLE_BACKWARD } else if newBrake < newOldBrakeForce { //手柄后退 msg.Type = message.RECIVE_TRAIN_HAND_KEY_BACKWARD } else { //手柄后退 msg.Type = message.RECIVE_TRAIN_HAND_KEY_BACKWARD }*/ msg.Type = message.RECIVE_TRAIN_HAND_KEY_BACKWARD } da := msg.Encode() //slog.Info("发送列车手柄消息", "msg", hex.EncodeToString(da)) err := trainClient.tcpClient.Send(da) //err := client.Send(da) if err != nil { slog.Error("发送列车手柄消息失败", "msg", hex.EncodeToString(da)) } } } func (d *trainPcSimService) SendTrainDirection(train *state_proto.TrainState, trainForward, trainBackward bool) { trainClient, trainDataErr := d.findTrainConn(train) if trainDataErr != nil { slog.Error(fmt.Sprintf("发送列车方向失败,未找到列车连接,trainId:%s", train.Id)) return } baseMsgs := make([]*message.TrainPcSimBaseMessage, 0) if !trainForward && !trainBackward { baseMsgs = append(baseMsgs, &message.TrainPcSimBaseMessage{Type: message.RECIVE_TRAIN_HAND_KEY_CANCLE_FORWARD}) baseMsgs = append(baseMsgs, &message.TrainPcSimBaseMessage{Type: message.RECIVE_TRAIN_HAND_KEY_CACLE_BACKWARD}) } else if trainForward { baseMsgs = append(baseMsgs, &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_HAND_KEY_FORWARD}) } else if trainBackward { baseMsgs = append(baseMsgs, &message.TrainPcSimBaseMessage{Type: message.RECIVE_TRAIN_HAND_KEY_BACKWARD}) } for _, msg := range baseMsgs { da := msg.Encode() //slog.Info(fmt.Sprintf("发送列车方向列车:%v ,数据:%v", train.Id, hex.EncodeToString(da))) err := trainClient.tcpClient.Send(da) if err != nil { slog.Error(fmt.Sprintf("发送列车方向失败列车:%v ,数据:%v,err:%v", train.Id, hex.EncodeToString(da), err.Error())) } } } func (d *trainPcSimService) SendBaliseData2(train *state_proto.TrainState, trainClientPort state_proto.TrainState_TrainPort, msgType byte, data []byte) { trainClient, trainDataErr := d.findTrainConnForPort2(train, trainClientPort) if trainDataErr != nil { slog.Error(fmt.Sprintf("发送列车PC仿真应答器信息失败,未找到列车连接,trainId:%v", train.Id)) return } msg := &message.TrainPcSimBaseMessage{} msg.Type = msgType msg.Data = data da := msg.Encode() //slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息,数据类型:0x%x,源数据长度:%v,数据:%v", msgType, len(data), hex.EncodeToString(da))) err := trainClient.tcpClient.Send(da) if err != nil { slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息失败,数据:%v", hex.EncodeToString(da))) } } func (d *trainPcSimService) SendTrainControlMsg2(train *state_proto.TrainState, baseMessage []message.TrainPcSimBaseMessage, trainClientPort state_proto.TrainState_TrainPort) { if len(baseMessage) <= 0 { return } //trainClient, trainDataErr := d.findTrainConn(train) trainClient, trainDataErr := d.findTrainConnForPort2(train, trainClientPort) if trainDataErr != nil { slog.Error(fmt.Sprintf("发送列车控制信息失败,无连接,列车Id:%v", train.Id)) return } for _, msg := range baseMessage { dd := msg.Encode() //slog.Info(fmt.Sprintf("发送操控列车控制信息:%x", dd), aport) d.sendData(trainClient.tcpClient, dd) } } func (d *trainPcSimService) sendData(client *tcp.TcpClient, data []byte) { err := client.Send(data) if err != nil { slog.Error(fmt.Sprintf("列车数字量信息发送失败,数据:%v", err.Error())) } }