From 163c0f53c28f894442f4653c75d3ed460961d3ad Mon Sep 17 00:00:00 2001 From: xzb <223@qq.com> Date: Tue, 5 Dec 2023 09:37:34 +0800 Subject: [PATCH] =?UTF-8?q?balise=20detect=20=E9=A2=84=E6=B5=8B=E5=AE=9E?= =?UTF-8?q?=E7=8E=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- third_party/can_btm/balise_btm.go | 123 ++++++++++++++---- third_party/can_btm/balise_detection.go | 110 ++++++++++------ .../wayside/memory/wayside_memory_train.go | 10 ++ 3 files changed, 178 insertions(+), 65 deletions(-) diff --git a/third_party/can_btm/balise_btm.go b/third_party/can_btm/balise_btm.go index 59efdf1..3c8e9f3 100644 --- a/third_party/can_btm/balise_btm.go +++ b/third_party/can_btm/balise_btm.go @@ -5,8 +5,9 @@ import ( "joylink.club/bj-rtsts-server/config" "joylink.club/bj-rtsts-server/third_party/message" "joylink.club/bj-rtsts-server/third_party/udp" - "joylink.club/bj-rtsts-server/ts/simulation/wayside/memory" - "joylink.club/rtsssimulation/fi" + "joylink.club/ecs" + "joylink.club/rtsssimulation/component" + "joylink.club/rtsssimulation/entity" "log/slog" "sort" "sync" @@ -17,7 +18,7 @@ import ( // BtmCanetManager BTM CANET 管理器 type BtmCanetManager interface { - memory.VerifyEvn + EvnWorld() ecs.World //GetBtmCanetConfig 获取CANET配置信息 GetBtmCanetConfig() config.BtmCanetConfig } @@ -48,6 +49,16 @@ type btmCanetClient struct { dsn byte //重发的数据 resendData *resendData + //车载BTM天线，探测应答器 + baliseDetector *BaliseDetector + //车载应答器天线功率放大器开关，true-开，false-关 + powerAmplifierSwitch bool + //天线此时是否在应答器上方 + aboveBalise bool + //应答器计数（每过一个应答器加一，在同一个应答器内不变） + baliseCounter int + //报文计数器（每解出一个应答器报文加一） + messageCounter int } type btmClock struct { btmTk uint32 //与ATP系统同步的时间ms @@ -62,6 +73,8 @@ func (c *btmClock) tkNow() uint32 { type BtmCanetClient interface { Start(bcm BtmCanetManager) Stop() + //HandleTrainHeadPositionInfo 处理收到列车位置信息 + HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo) } var ( @@ -73,11 +86,32 @@ func Default() BtmCanetClient { btmClientLocker.Lock() defer btmClientLocker.Unlock() if btmClient == nil { - btmClient = &btmCanetClient{} + btmClient = &btmCanetClient{baliseDetector: &BaliseDetector{}} } return btmClient } +// 应答器计数器加1，[0,255] +func (t *btmCanetClient) baliseCounterAdd1() { + t.baliseCounter++ + if t.baliseCounter > 255 { + t.baliseCounter = 0 + } +} + +// 报文计数器加1，[0,255] +func (t *btmCanetClient) baliseMessageCounterAdd1() { + t.messageCounter++ + if t.messageCounter > 255 { + t.messageCounter = 0 + } +} +func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo) { + //slog.Debug(h.String()) + wd := entity.GetWorldData(w) + repo := wd.Repo + s.baliseDetector.Detect(wd, repo, h) +} func (s *btmCanetClient) Start(bcm BtmCanetManager) { s.bcm = bcm cfg := s.bcm.GetBtmCanetConfig() @@ -176,36 +210,65 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) { s.btmTime.sysTk = time.Now() s.atpReqSn = atpReq.FId.ID4 s.atpReqCrc16Check = atpReq.Crc16CheckOk - se := fi.TrainBalisePowerAmplifierSwitch(s.bcm.EvnWorld(), atpReq.PowerAmplifierTurnOn) - if se != nil { - //slog.Warn(fmt.Sprintf("列车车载BTM功率放大器开关控制异常[%s]", se.Error())) - } - //ATP 是否要求BTM 重发上一应答器报文 - isResendRequest := atpReq.ResendRequest == 2 //0b10 - s.rspToAtp(isResendRequest) + s.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn //记录atp查询时间 s.atpReqTime = &now + //ATP 是否要求BTM 重发上一应答器报文 + isResendRequest := atpReq.ResendRequest == 2 //0b10 + s.aboveBalise = s.baliseDetector.HasBaliseBeingScan() + if isResendRequest { + s.rspResendToAtp() + } else { + sb := s.baliseDetector.DoScan() + if sb != nil { + slog.Debug(fmt.Sprintf("BTM经过应答器[%s],BTM与ATP时间差[%d]ms", sb.BaliseId, time.Now().UnixMilli()-sb.Time.UnixMilli())) + s.rspToAtp(sb) + } + } +} + +func (s *btmCanetClient) createTrainBtmStatus() *TrainBtmStatus { + return &TrainBtmStatus{ + PowerAmplifierOn: s.powerAmplifierSwitch, + PowerAmplifierFault: false, + AboveBalise: s.aboveBalise, + AntennaFault: false, + BaliseCounter: s.baliseCounter, + MessageCounter: s.messageCounter, + } +} + +// 获取应答器报文(线程不安全) +func (s *btmCanetClient) findBaliseTelegram(baliseId string) []byte { + wd := entity.GetWorldData(s.bcm.EvnWorld()) + entry, ok := wd.EntityMap[baliseId] + if !ok { + return nil + } else { + return component.BaliseStateType.Get(entry).ValidTelegram + } } // BTM发送响应给ATP // 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧 // 当未收到应答器报文时响应：时间同步帧、状态应答帧 -func (s *btmCanetClient) rspToAtp(isResendRequest bool) { +func (s *btmCanetClient) rspResendToAtp() { //重发上一报文处理 - if isResendRequest && s.resendData != nil && s.resendData.canResend() { + if s.resendData != nil && s.resendData.canResend() { s.resendData.countAdd1() s.sendCanetFrame(s.resendData.data) return - } else { - s.resendData = nil } +} + +// BTM发送响应给ATP +// 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧 +// 当未收到应答器报文时响应：时间同步帧、状态应答帧 +func (s *btmCanetClient) rspToAtp(sb *BtmAntennaScanningBaliseInfo) { //BTM状态 statusF := message.NewBtmStatusRspFrame(s.atpReqSn) - btmStatus, btmStatusErr := fi.FindTrainBaliseBtmStatus(s.bcm.EvnWorld()) - if btmStatusErr != nil { - //slog.Debug(fmt.Sprintf("从仿真获取BTM状态失败：%s", btmStatusErr.Error())) - return - } + btmStatus := s.createTrainBtmStatus() + statusF.AntennaFault = btmStatus.AntennaFault statusF.BaliseCounter = byte(btmStatus.BaliseCounter) statusF.MessageCounter = byte(btmStatus.MessageCounter) @@ -220,16 +283,11 @@ func (s *btmCanetClient) rspToAtp(isResendRequest bool) { statusF.Dsn = s.dsn s.dsnAdd1() // - baliseTelegram, btSendErr := fi.GetScannedBaliseTelegram(s.bcm.EvnWorld()) - if btSendErr != nil { - //slog.Debug(btSendErr.Error()) - } else { - slog.Debug(fmt.Sprintf("发送应答器报文，应答器：[%s]", baliseTelegram.BaliseId)) - } + baliseTelegram := s.findBaliseTelegram(sb.BaliseId) //true-收到应答器报文 isRcvTelegram := baliseTelegram != nil if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧 - statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram.Telegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow()) + statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow()) if statusDataCfOk { timeSyncF := message.NewBtmTimeSyncCheckFrame(s.atpReqSn) timeSyncF.T2 = s.btmTime.btmTk @@ -301,3 +359,14 @@ func (s *btmCanetClient) dealWithBtmDataFrames(dms []*message.CanetFrame) { slog.Debug(fmt.Sprintf("接收到数据帧%s", dm.String())) } } + +////////////////////////////////////////////////////////////////////// + +type TrainBtmStatus struct { + PowerAmplifierOn bool //BTM功率放大器是否开启 + PowerAmplifierFault bool //BTM功率放大器是否有故障 + AntennaFault bool //BTM应答器天线是否有故障 + AboveBalise bool //BTM当前是否在应答器上方 + BaliseCounter int //应答器计数（每过一个应答器加一，在同一个应答器内不变） + MessageCounter int //报文计数器（每解出一个应答器报文加一） +} diff --git a/third_party/can_btm/balise_detection.go b/third_party/can_btm/balise_detection.go index 2060f24..a5fa370 100644 --- a/third_party/can_btm/balise_detection.go +++ b/third_party/can_btm/balise_detection.go @@ -3,9 +3,9 @@ package can_btm import ( "fmt" "joylink.club/rtsssimulation/component" + "joylink.club/rtsssimulation/fi" "joylink.club/rtsssimulation/repository" "joylink.club/rtsssimulation/repository/model/proto" - "log/slog" "math" "sort" "sync" @@ -13,6 +13,7 @@ import ( ) //通过提前预测来实现BTM天线扫描应答器的实时性 +//根据当前列车运行信息预测出列车前方应答器被扫描到的时刻 // BtmAntennaRunningInfo 车载BTM天线中心点位置运行信息 // BTM天线一般在第一车轴后某个位置 @@ -29,20 +30,7 @@ const ( ) // TrainHeadPositionInfo 列车车头运行位置信息 -type TrainHeadPositionInfo struct { - //列车id - TrainId string - //列车头当前运行方向（true偏移量增大/false减小方向） - Up bool - //列车所在轨道link - Link string - //列车所在link偏移量（mm） - LinkOffset int64 - //列车运行速度（m/s） - Speed float32 - //加速度(m/s^2) - Acceleration float32 -} +type TrainHeadPositionInfo = fi.TrainHeadPositionInfo type BtmAntennaToBaliseInfo struct { Distance int64 //BTM天线中心到应答器的距离，mm BaliseId string //应答器id @@ -50,41 +38,78 @@ type BtmAntennaToBaliseInfo struct { type BtmAntennaScanningBaliseInfo struct { BaliseId string //应答器id Time time.Time //应答器预计被BTM天线激活的时刻 + active bool //true-激活过，即列车扫过 } + +// BaliseDetector 车载BTM天线，应答器探测器 type BaliseDetector struct { - basbi *BtmAntennaScanningBaliseInfo - basbiLock sync.Mutex + eq [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头 + eqLock sync.Mutex + + last *BtmAntennaScanningBaliseInfo } func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) { curTime := time.Now() - bari := t.createBtmAntennaRunningInfo(wd, repo, th) + //BTM天线中心点运行信息 + curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th) //预测BTM天线到最近一个应答器的时刻 - basbi := t.timeScanNearestBalise(curTime, wd, repo, bari) - if basbi != nil { - t.updateBasbi(basbi) - slog.Debug("将要激活应答器", "BaliseId", basbi.BaliseId, "ActiveTime", basbi.Time) + curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi) + if curExpect != nil { + dt := curExpect.Time.UnixMilli() - curTime.UnixMilli() + if dt <= 20 { + //slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt) + t.addExpectedBalise(curExpect) + } } } -func (t *BaliseDetector) ScanBaliseByTime(curTime time.Time) *BtmAntennaScanningBaliseInfo { - t.basbiLock.Lock() - defer t.basbiLock.Unlock() - // - if t.basbi != nil && curTime.UnixMilli() >= t.basbi.Time.UnixMilli() { - rt := *t.basbi - return &rt +func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) { + if curExpect == nil { + return } - return nil + // + t.eqLock.Lock() + defer t.eqLock.Unlock() + //更新 + for i, e := range t.eq { + if e != nil && e.BaliseId == curExpect.BaliseId { + t.eq[i].Time = curExpect.Time + return + } + } + //左移 + for i := 1; i < len(t.eq); i++ { + t.eq[i-1] = t.eq[i] + } + //存入队尾 + t.eq[len(t.eq)-1] = curExpect } -func (t *BaliseDetector) updateBasbi(basbi *BtmAntennaScanningBaliseInfo) { - t.basbiLock.Lock() - defer t.basbiLock.Unlock() +func (t *BaliseDetector) DoScan() *BtmAntennaScanningBaliseInfo { // - if basbi != nil { - *t.basbi = *basbi - } else { - t.basbi = nil + t.eqLock.Lock() + defer t.eqLock.Unlock() + // + var rt *BtmAntennaScanningBaliseInfo + for i := 0; i < len(t.eq); i++ { + if t.eq[i] != nil && !t.eq[i].active { + rt = t.eq[i] + t.eq[i].active = true + break + } } + return rt +} +func (t *BaliseDetector) HasBaliseBeingScan() bool { + // + t.eqLock.Lock() + defer t.eqLock.Unlock() + // + for i := 0; i < len(t.eq); i++ { + if t.eq[i] != nil && !t.eq[i].active { + return true + } + } + return false } // 计算列车在当前运行状态下，预测到最近一个应答器的时刻 @@ -225,6 +250,10 @@ func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.R curLink := repo.FindLink(curLinkId) if searchDirection { bDc := curLink.BRelation() + if bDc == nil || bDc.Turnout() == nil { + return nil + } + // bDcDw := t.turnoutPosition(wd, bDc.Turnout().Id()) switch bDc.Port() { case proto.Port_A: //link-b连接turnout-A @@ -240,6 +269,10 @@ func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.R } } else { aDc := curLink.ARelation() + if aDc == nil || aDc.Turnout() == nil { + return nil + } + // aDcDw := t.turnoutPosition(wd, aDc.Turnout().Id()) switch aDc.Port() { case proto.Port_A: //link-a连接turnout-A @@ -283,7 +316,8 @@ func (t *BaliseDetector) calculateQuadratic(a, b, c float64) (float64, bool) { x1 := (-b + sqrtD) / ax2 x2 := (-b - sqrtD) / ax2 if x1 >= 0 && x2 >= 0 { - panic("x1 >= 0 && x2 >= 0") + //panic("x1 >= 0 && x2 >= 0") + return 0, false } if x1 >= 0 { return x1, true diff --git a/ts/simulation/wayside/memory/wayside_memory_train.go b/ts/simulation/wayside/memory/wayside_memory_train.go index 69c2fe7..2876ae3 100644 --- a/ts/simulation/wayside/memory/wayside_memory_train.go +++ b/ts/simulation/wayside/memory/wayside_memory_train.go @@ -2,6 +2,7 @@ package memory import ( "fmt" + "joylink.club/bj-rtsts-server/third_party/can_btm" "log/slog" "math" "strconv" @@ -126,6 +127,15 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess panic(sys_error.New("动力学传输数据:列车车尾位置计算出错", e2)) } //slog.Debug("车尾位置", tailDeviceId, "偏移", tailDeviceOffset, "所在设备端", tailDevicePort) + // 更新BTM中列车位置信息 + can_btm.Default().HandleTrainHeadPositionInfo(vs.World, &fi.TrainHeadPositionInfo{ + TrainId: trainId, + Up: info.Up, + Link: outLinkId, + LinkOffset: outLinkOffset, + Speed: info.Speed, + Acceleration: info.Acceleration, + }) // 修改world中的列车位置 fi.UpdateTrainPositionFromDynamics(vs.World, fi.TrainPositionInfo{ TrainId: trainId,