322 lines
11 KiB
Go
322 lines
11 KiB
Go
package simulation
|
|
|
|
import (
|
|
"encoding/binary"
|
|
"encoding/hex"
|
|
"math"
|
|
"time"
|
|
|
|
"fmt"
|
|
"net/http"
|
|
"strconv"
|
|
"sync"
|
|
|
|
"go.uber.org/zap"
|
|
"joylink.club/bj-rtsts-server/ats/verify/protos/state"
|
|
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory"
|
|
"joylink.club/bj-rtsts-server/config"
|
|
"joylink.club/bj-rtsts-server/dynamics"
|
|
"joylink.club/bj-rtsts-server/vobc"
|
|
|
|
"joylink.club/bj-rtsts-server/dto"
|
|
)
|
|
|
|
func init() {
|
|
// vobc 发来的列车信息
|
|
vobc.RegisterTrainInfoHandler(func(info []byte) {
|
|
zap.S().Info("接收到vobc发送的列车消息", hex.EncodeToString(info))
|
|
memory.UdpUpdateTime.VobcTime = time.Now().UnixMilli()
|
|
for _, simulation := range GetSimulationArr() {
|
|
simulation.Memory.Status.TrainStateMap.Range(func(_, value any) bool {
|
|
train := value.(*state.TrainState)
|
|
if !train.Show { // 下线列车
|
|
return false
|
|
}
|
|
// 拼接列车ID
|
|
trainId, err := strconv.Atoi(train.Id)
|
|
if err != nil {
|
|
panic(dto.ErrorDto{Code: dto.ArgumentParseError, Message: err.Error()})
|
|
}
|
|
trainInfo := decoderVobcTrainState(info)
|
|
zap.S().Info("解析后vobc列车消息", trainInfo)
|
|
d := append(info, uint8(trainId))
|
|
// 发送给动力学
|
|
dynamics.SendDynamicsTrainMsg(d)
|
|
zap.S().Info("发送后：", hex.EncodeToString(d))
|
|
// 存放至列车中
|
|
train.VobcState = trainInfo
|
|
return true
|
|
})
|
|
}
|
|
})
|
|
dynamics.RegisterTrainInfoHandler(func(info *dynamics.TrainInfo) {
|
|
zap.S().Info("发送到vobc的列车编号为", info.Number)
|
|
memory.UdpUpdateTime.DynamicsTime = time.Now().UnixMilli()
|
|
for _, simulation := range GetSimulationArr() {
|
|
sta, ok := simulation.Memory.Status.TrainStateMap.Load(strconv.Itoa(int(info.Number)))
|
|
if !ok {
|
|
continue
|
|
}
|
|
trainState := sta.(*state.TrainState)
|
|
// 给半实物仿真发送速度
|
|
zap.S().Infof("发送到vobc发送的速度%d, %d \n", info.Speed, math.Abs(float64(info.Speed*36)))
|
|
vobc.SendTrainSpeedTask(math.Abs(float64(info.Speed * 36)))
|
|
// 更新列车状态
|
|
memory.UpdateTrainState(simulation, convert(info, trainState, simulation))
|
|
}
|
|
})
|
|
}
|
|
|
|
// 仿真存储集合
|
|
var simulationMap sync.Map
|
|
|
|
// 创建前检查
|
|
func IsExistSimulation() bool {
|
|
i := 0
|
|
simulationMap.Range(func(_, _ any) bool {
|
|
i++
|
|
return true
|
|
})
|
|
return i > 0
|
|
}
|
|
|
|
// 创建仿真对象
|
|
func CreateSimulation(mapId, projectId int32) string {
|
|
var simulationId string
|
|
if projectId == 0 {
|
|
simulationId = createSimulationId(mapId)
|
|
} else {
|
|
simulationId = createSimulationIdByProject(projectId)
|
|
}
|
|
_, e := simulationMap.Load(simulationId)
|
|
if !e && IsExistSimulation() {
|
|
panic(dto.ErrorDto{Code: dto.DataAlreadyExist, Message: "已有仿真在运行"})
|
|
}
|
|
if !e {
|
|
verifySimulation := memory.CreateSimulation(mapId, projectId, simulationId)
|
|
//通知动力学
|
|
httpCode, _, err := dynamics.SendSimulationStartReq(buildLineBaseInfo(memory.QueryMapVerifyStructure(verifySimulation.MapId)))
|
|
if httpCode != http.StatusOK || err != nil {
|
|
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
|
|
}
|
|
simulationMap.Store(simulationId, verifySimulation)
|
|
dynamicsRun(verifySimulation)
|
|
}
|
|
return simulationId
|
|
}
|
|
|
|
// 删除仿真对象
|
|
func DestroySimulation(simulationId string) {
|
|
simulationMap.Delete(simulationId)
|
|
//移除道岔状态发送
|
|
dynamics.Stop()
|
|
//通知动力学
|
|
httpCode, _, err := dynamics.SendSimulationEndReq()
|
|
if httpCode != http.StatusOK || err != nil {
|
|
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
|
|
}
|
|
}
|
|
|
|
// 创建时生成仿真Id
|
|
func createSimulationId(mapId int32) string {
|
|
// 当前服务器IP + 端口 + 地图
|
|
return config.SimulationId_prefix + "_" + strconv.Itoa(config.Config.Server.Port) + "_map_" + strconv.Itoa(int(mapId))
|
|
}
|
|
|
|
func createSimulationIdByProject(mapId int32) string {
|
|
// 当前服务器IP + 端口 + 地图
|
|
return config.SimulationId_prefix + "_" + strconv.Itoa(config.Config.Server.Port) + "_project_" + strconv.Itoa(int(mapId))
|
|
}
|
|
|
|
// 获取仿真列表
|
|
func ListAllSimulations() []*dto.SimulationInfoRspDto {
|
|
var simArr []*dto.SimulationInfoRspDto
|
|
simulationMap.Range(func(_, v any) bool {
|
|
s := v.(*memory.VerifySimulation)
|
|
simArr = append(simArr, &dto.SimulationInfoRspDto{
|
|
SimulationId: s.SimulationId,
|
|
MapId: s.MapId,
|
|
ProjectId: s.ProjectId,
|
|
})
|
|
return true
|
|
})
|
|
return simArr
|
|
}
|
|
|
|
// 根据仿真id查找仿真实例
|
|
func FindSimulation(simulationId string) *memory.VerifySimulation {
|
|
m, e := simulationMap.Load(simulationId)
|
|
if e {
|
|
return m.(*memory.VerifySimulation)
|
|
}
|
|
return nil
|
|
}
|
|
|
|
// 获取普通仿真数组
|
|
func GetSimulationArr() []*memory.VerifySimulation {
|
|
var result []*memory.VerifySimulation
|
|
simulationMap.Range(func(_, v any) bool {
|
|
result = append(result, v.(*memory.VerifySimulation))
|
|
return true
|
|
})
|
|
return result
|
|
}
|
|
|
|
func convert(info *dynamics.TrainInfo, sta *state.TrainState, simulation *memory.VerifySimulation) *state.TrainState {
|
|
vs := memory.QueryMapVerifyStructure(simulation.MapId)
|
|
zap.S().Debugf("原始消息：[%d-%d-%d]", info.Number, info.Link, info.LinkOffset)
|
|
id, port, offset, runDirection, pointTo, kilometer := memory.QueryDeviceByCalcLink(vs, int32(info.Link), int64(info.LinkOffset), info.Up, sta.RunDirection)
|
|
zap.S().Debugf("转换后的消息：[%d-车头:%s-偏移:%d-上行:%v-是否ab:%v]", info.Number, id, offset, runDirection, pointTo)
|
|
sta.HeadDeviceId = id
|
|
sta.DevicePort = port
|
|
sta.HeadOffset = offset
|
|
sta.PointTo = pointTo
|
|
sta.TrainKilometer = kilometer
|
|
sta.RunDirection = runDirection
|
|
//判定车头方向
|
|
sta.HeadDirection = runDirection
|
|
if sta.VobcState != nil {
|
|
if sta.VobcState.DirectionForward {
|
|
sta.HeadDirection = runDirection
|
|
} else if sta.VobcState.DirectionBackward {
|
|
sta.HeadDirection = !runDirection
|
|
}
|
|
}
|
|
if info.Speed < 0 {
|
|
sta.RunDirection = !sta.RunDirection
|
|
}
|
|
// 赋值动力学信息
|
|
sta.DynamicState.Heartbeat = int32(info.LifeSignal)
|
|
sta.DynamicState.HeadLinkId = strconv.Itoa(int(info.Link))
|
|
sta.DynamicState.HeadLinkOffset = int64(info.LinkOffset)
|
|
sta.DynamicState.Slope = int32(info.Slope)
|
|
sta.DynamicState.Upslope = info.UpSlope
|
|
sta.DynamicState.RunningUp = info.Up
|
|
sta.DynamicState.RunningResistanceSum = float32(info.TotalResistance) / 1000
|
|
sta.DynamicState.AirResistance = float32(info.AirResistance) / 1000
|
|
sta.DynamicState.RampResistance = float32(info.SlopeResistance) / 1000
|
|
sta.DynamicState.CurveResistance = float32(info.CurveResistance) / 1000
|
|
sta.DynamicState.Speed = speedParse(info.Speed)
|
|
sta.DynamicState.HeadSensorSpeed1 = speedParse(info.HeadSpeed1)
|
|
sta.DynamicState.HeadSensorSpeed2 = speedParse(info.HeadSpeed2)
|
|
sta.DynamicState.TailSensorSpeed1 = speedParse(info.TailSpeed1)
|
|
sta.DynamicState.TailSensorSpeed2 = speedParse(info.TailSpeed2)
|
|
sta.DynamicState.HeadRadarSpeed = speedParse(info.HeadRadarSpeed)
|
|
sta.DynamicState.TailRadarSpeed = speedParse(info.TailRadarSpeed)
|
|
return sta
|
|
}
|
|
|
|
func dynamicsRun(verifySimulation *memory.VerifySimulation) {
|
|
_ = dynamics.Run(func() []*dynamics.TurnoutInfo {
|
|
stateSlice := memory.GetAllTurnoutState(verifySimulation)
|
|
var turnoutInfoSlice []*dynamics.TurnoutInfo
|
|
for _, sta := range stateSlice {
|
|
code64, err := strconv.ParseUint(sta.Id, 10, 16)
|
|
if err != nil {
|
|
zap.S().Error("id转uint16报错", err)
|
|
}
|
|
info := dynamics.TurnoutInfo{
|
|
Code: uint16(code64),
|
|
NPosition: sta.Normal,
|
|
RPosition: sta.Reverse,
|
|
}
|
|
turnoutInfoSlice = append(turnoutInfoSlice, &info)
|
|
}
|
|
return turnoutInfoSlice
|
|
})
|
|
}
|
|
|
|
func buildLineBaseInfo(vs *memory.VerifyStructure) *dynamics.LineBaseInfo {
|
|
var links []*dynamics.Link
|
|
var slopes []*dynamics.Slope
|
|
var curves []*dynamics.Curve
|
|
for _, link := range vs.LinkModelMap {
|
|
id, _ := strconv.Atoi(link.Index)
|
|
var aTurnoutId int
|
|
var aPort string
|
|
if link.ARelatedSwitchRef != nil && link.ARelatedSwitchRef.SwitchDevice != nil {
|
|
aTurnoutId, _ = strconv.Atoi(link.ARelatedSwitchRef.SwitchDevice.GetIndex())
|
|
aPort = link.ARelatedSwitchRef.Port.Name()
|
|
}
|
|
var bTurnoutId int
|
|
var bPort string
|
|
if link.BRelatedSwitchRef != nil && link.BRelatedSwitchRef.SwitchDevice != nil {
|
|
bTurnoutId, _ = strconv.Atoi(link.BRelatedSwitchRef.SwitchDevice.GetIndex())
|
|
bPort = link.BRelatedSwitchRef.Port.Name()
|
|
}
|
|
links = append(links, &dynamics.Link{
|
|
ID: int32(id),
|
|
Len: link.Length,
|
|
ARelTurnoutId: int32(aTurnoutId),
|
|
ARelTurnoutPoint: aPort,
|
|
BRelTurnoutId: int32(bTurnoutId),
|
|
BRelTurnoutPoint: bPort,
|
|
})
|
|
}
|
|
for _, slope := range vs.SlopeModelMap {
|
|
id, _ := strconv.Atoi(slope.Index)
|
|
slopes = append(slopes, &dynamics.Slope{
|
|
ID: int32(id),
|
|
StartLinkId: slope.StartLinkIndex,
|
|
StartLinkOffset: slope.StartLinkOffset,
|
|
EndLinkId: slope.EndLinkIndex,
|
|
EndLinkOffset: slope.EndLinkOffset,
|
|
DegreeTrig: slope.DegreeTrig,
|
|
})
|
|
}
|
|
for _, curve := range vs.CurveModelMap {
|
|
id, _ := strconv.Atoi(curve.Index)
|
|
curves = append(curves, &dynamics.Curve{
|
|
ID: int32(id),
|
|
StartLinkId: curve.StartLinkIndex,
|
|
StartLinkOffset: curve.StartLinkOffset,
|
|
EndLinkId: curve.EndLinkIndex,
|
|
EndLinkOffset: curve.EndLinkOffset,
|
|
Curvature: curve.Curvature,
|
|
})
|
|
}
|
|
return &dynamics.LineBaseInfo{
|
|
LinkList: links,
|
|
SlopeList: slopes,
|
|
CurveList: curves,
|
|
}
|
|
}
|
|
|
|
// 解析VOBC列车信息
|
|
func decoderVobcTrainState(buf []byte) *state.TrainVobcState {
|
|
trainVobcInfo := &state.TrainVobcState{}
|
|
trainVobcInfo.LifeSignal = int32(binary.BigEndian.Uint16(buf[0:2]))
|
|
b2 := buf[2]
|
|
trainVobcInfo.Tc1Active = (b2 & 1) != 0
|
|
trainVobcInfo.Tc2Active = (b2 & (1 << 1)) != 0
|
|
trainVobcInfo.DirectionForward = (b2 & (1 << 2)) != 0
|
|
trainVobcInfo.DirectionBackward = (b2 & (1 << 3)) != 0
|
|
trainVobcInfo.TractionStatus = (b2 & (1 << 4)) != 0
|
|
trainVobcInfo.BrakingStatus = (b2 & (1 << 5)) != 0
|
|
trainVobcInfo.EmergencyBrakingStatus = (b2 & (1 << 6)) != 0
|
|
trainVobcInfo.TurnbackStatus = (b2 & 7) != 0
|
|
b3 := buf[3]
|
|
trainVobcInfo.JumpStatus = (b3 & 1) != 0
|
|
trainVobcInfo.Ato = (b3 & (1 << 1)) != 0
|
|
trainVobcInfo.Fam = (b3 & (1 << 2)) != 0
|
|
trainVobcInfo.Cam = (b3 & (1 << 3)) != 0
|
|
trainVobcInfo.TractionSafetyCircuit = (b3 & (1 << 4)) != 0
|
|
trainVobcInfo.ParkingBrakeStatus = (b3 & (1 << 5)) != 0
|
|
trainVobcInfo.MaintainBrakeStatus = (b3 & (1 << 6)) != 0
|
|
trainVobcInfo.TractionForce = int64(binary.BigEndian.Uint16(buf[4:6]))
|
|
trainVobcInfo.BrakeForce = int64(binary.BigEndian.Uint16(buf[6:8]))
|
|
trainVobcInfo.TrainLoad = int64(binary.BigEndian.Uint16(buf[8:10]))
|
|
b4 := buf[15]
|
|
trainVobcInfo.LeftDoorOpenCommand = (b4 & 1) != 0
|
|
trainVobcInfo.RightDoorOpenCommand = (b4 & (1 << 1)) != 0
|
|
trainVobcInfo.LeftDoorCloseCommand = (b4 & (1 << 2)) != 0
|
|
trainVobcInfo.RightDoorCloseCommand = (b4 & (1 << 3)) != 0
|
|
trainVobcInfo.AllDoorClose = (b4 & (1 << 4)) != 0
|
|
return trainVobcInfo
|
|
}
|
|
|
|
// 发送给前端的速度格式化
|
|
func speedParse(speed float32) int32 {
|
|
return int32(math.Abs(float64(speed * 3.6 * 100)))
|
|
}
|