From 83a5ee52e8a2eaf6d963fbad29f8a73e9de032b6 Mon Sep 17 00:00:00 2001 From: xzb <223@qq.com> Date: Tue, 5 Dec 2023 10:57:31 +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 | 54 ++++--------------- third_party/can_btm/balise_detection.go | 69 ++++++++++++++++++++----- 2 files changed, 66 insertions(+), 57 deletions(-) diff --git a/third_party/can_btm/balise_btm.go b/third_party/can_btm/balise_btm.go index 3c8e9f3..123793c 100644 --- a/third_party/can_btm/balise_btm.go +++ b/third_party/can_btm/balise_btm.go @@ -6,7 +6,6 @@ import ( "joylink.club/bj-rtsts-server/third_party/message" "joylink.club/bj-rtsts-server/third_party/udp" "joylink.club/ecs" - "joylink.club/rtsssimulation/component" "joylink.club/rtsssimulation/entity" "log/slog" "sort" @@ -51,14 +50,6 @@ type btmCanetClient struct { resendData *resendData //车载BTM天线，探测应答器 baliseDetector *BaliseDetector - //车载应答器天线功率放大器开关，true-开，false-关 - powerAmplifierSwitch bool - //天线此时是否在应答器上方 - aboveBalise bool - //应答器计数（每过一个应答器加一，在同一个应答器内不变） - baliseCounter int - //报文计数器（每解出一个应答器报文加一） - messageCounter int } type btmClock struct { btmTk uint32 //与ATP系统同步的时间ms @@ -90,22 +81,6 @@ func Default() BtmCanetClient { } 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) @@ -210,12 +185,11 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) { s.btmTime.sysTk = time.Now() s.atpReqSn = atpReq.FId.ID4 s.atpReqCrc16Check = atpReq.Crc16CheckOk - s.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn + s.baliseDetector.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn //记录atp查询时间 s.atpReqTime = &now //ATP 是否要求BTM 重发上一应答器报文 isResendRequest := atpReq.ResendRequest == 2 //0b10 - s.aboveBalise = s.baliseDetector.HasBaliseBeingScan() if isResendRequest { s.rspResendToAtp() } else { @@ -223,29 +197,20 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) { if sb != nil { slog.Debug(fmt.Sprintf("BTM经过应答器[%s],BTM与ATP时间差[%d]ms", sb.BaliseId, time.Now().UnixMilli()-sb.Time.UnixMilli())) s.rspToAtp(sb) + } else { + s.rspToAtp(nil) } } } func (s *btmCanetClient) createTrainBtmStatus() *TrainBtmStatus { return &TrainBtmStatus{ - PowerAmplifierOn: s.powerAmplifierSwitch, + PowerAmplifierOn: s.baliseDetector.powerAmplifierSwitch, PowerAmplifierFault: false, - AboveBalise: s.aboveBalise, + AboveBalise: s.baliseDetector.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 + BaliseCounter: s.baliseDetector.baliseCounter, + MessageCounter: s.baliseDetector.messageCounter, } } @@ -283,11 +248,10 @@ func (s *btmCanetClient) rspToAtp(sb *BtmAntennaScanningBaliseInfo) { statusF.Dsn = s.dsn s.dsnAdd1() // - baliseTelegram := s.findBaliseTelegram(sb.BaliseId) //true-收到应答器报文 - isRcvTelegram := baliseTelegram != nil + isRcvTelegram := sb != nil && len(sb.telegram) > 0 if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧 - statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow()) + statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, sb.telegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow()) if statusDataCfOk { timeSyncF := message.NewBtmTimeSyncCheckFrame(s.atpReqSn) timeSyncF.T2 = s.btmTime.btmTk diff --git a/third_party/can_btm/balise_detection.go b/third_party/can_btm/balise_detection.go index a5fa370..26d5705 100644 --- a/third_party/can_btm/balise_detection.go +++ b/third_party/can_btm/balise_detection.go @@ -39,42 +39,86 @@ type BtmAntennaScanningBaliseInfo struct { BaliseId string //应答器id Time time.Time //应答器预计被BTM天线激活的时刻 active bool //true-激活过，即列车扫过 + telegram []byte //应答器报文 } // BaliseDetector 车载BTM天线，应答器探测器 type BaliseDetector struct { eq [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头 eqLock sync.Mutex - - last *BtmAntennaScanningBaliseInfo + //车载应答器天线功率放大器开关，true-开，false-关 + powerAmplifierSwitch bool + //天线此时是否在应答器上方 + aboveBalise bool + //应答器计数（每过一个应答器加一，在同一个应答器内不变） + baliseCounter int + //报文计数器（每解出一个应答器报文加一,应答器报文长度830bits） + messageCounter int } func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) { + if !t.powerAmplifierSwitch { //天线功率放大器未不进行探测 + return + } curTime := time.Now() //BTM天线中心点运行信息 curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th) //预测BTM天线到最近一个应答器的时刻 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) + if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms + //slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt) + //记录即将经过的应答器 + if t.addExpectedBalise(curExpect) { + t.baliseCounterAdd1() //应答器计数器 + telegram := t.rcvTelegram(wd, curExpect.BaliseId) + if len(telegram) > 0 { + curExpect.telegram = telegram + t.baliseMessageCounterAdd1() //报文计数器 + } } + //BTM天线即将经过应答器 + t.aboveBalise = true + } else { + t.aboveBalise = false } } -func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) { - if curExpect == nil { - return + +// 应答器计数器加1，[0,255] +func (t *BaliseDetector) baliseCounterAdd1() { + t.baliseCounter++ + if t.baliseCounter > 255 { + t.baliseCounter = 0 } +} + +// 报文计数器加1，[0,255] +func (t *BaliseDetector) baliseMessageCounterAdd1() { + t.messageCounter++ + if t.messageCounter > 255 { + t.messageCounter = 0 + } +} + +// BTM天线接收应答器报文(线程不安全) +func (t *BaliseDetector) rcvTelegram(wd *component.WorldData, baliseId string) []byte { + entry, ok := wd.EntityMap[baliseId] + if !ok { + return nil + } else { + return component.BaliseStateType.Get(entry).ValidTelegram + } +} + +// true-新增；false-更新 +func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) bool { // 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 + return false } } //左移 @@ -83,6 +127,7 @@ func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseIn } //存入队尾 t.eq[len(t.eq)-1] = curExpect + return true } func (t *BaliseDetector) DoScan() *BtmAntennaScanningBaliseInfo { //