From ed536bd92da228a7326276dfb8649c9677de5c12 Mon Sep 17 00:00:00 2001 From: xzb <223@qq.com> Date: Tue, 28 Nov 2023 16:05:41 +0800 Subject: [PATCH] btm --- component/train.go | 2 - fi/train.go | 7 +- sys/device_sys/balise_detection.go | 175 ++++++++++++++++------------- 3 files changed, 106 insertions(+), 78 deletions(-) diff --git a/component/train.go b/component/train.go index 22f70dc..9df1b4c 100644 --- a/component/train.go +++ b/component/train.go @@ -84,8 +84,6 @@ func (t *TrainBtm) SetBaliseTelegramHadSentAndGet() *TrainBaliseTelegram { return nil } -const scannedBalisesMax = 3 - // Scanning BTM通过车载应答器天线接收到应答器报文 func (t *TrainBtm) Scanning(aboveBalise bool, aboveBaliseId string, telegram *TrainBaliseTelegram) { t.AboveBalise = aboveBalise diff --git a/fi/train.go b/fi/train.go index c4f16ce..70bc718 100644 --- a/fi/train.go +++ b/fi/train.go @@ -43,14 +43,19 @@ func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error { te, find := wd.EntityMap[tpi.TrainId] if find { train := component.TrainPositionInfoType.Get(te) + // + lastTrainHeadPos := &device_sys.TrainHeadPosition{Up: train.Up, HeadLink: train.HeadLink, HeadLinkOffset: train.HeadLinkOffset} + // train.Up = tpi.Up train.Len = int64(tpi.Len) train.HeadLink = tpi.HeadLink train.HeadLinkOffset = int64(tpi.HeadLinkOffset) train.TailLink = tpi.TailLink train.TailLinkOffset = int64(tpi.TailLinkOffset) + //列车车头移动范围 + curTrainHeadPos := &device_sys.TrainHeadPosition{Up: train.Up, HeadLink: train.HeadLink, HeadLinkOffset: train.HeadLinkOffset} //根据列车位置探测应答器 - device_sys.NewBaliseDetection().DetectBalise(w, te) + device_sys.NewBaliseDetection().DetectBalise(w, te, lastTrainHeadPos, curTrainHeadPos) // return ecs.NewOkEmptyResult() } else { diff --git a/sys/device_sys/balise_detection.go b/sys/device_sys/balise_detection.go index 04838e4..47b73d8 100644 --- a/sys/device_sys/balise_detection.go +++ b/sys/device_sys/balise_detection.go @@ -3,12 +3,10 @@ package device_sys import ( "fmt" "joylink.club/ecs" - "joylink.club/ecs/filter" "joylink.club/rtsssimulation/component" "joylink.club/rtsssimulation/entity" "joylink.club/rtsssimulation/repository" "log/slog" - "math" "sort" "strings" ) @@ -17,59 +15,111 @@ import ( // 一条应答器报文长830bits;单个应答器对应多条报文； // 固定应答器对应1条报文；默认报文对应1条； type BaliseDetectSystem struct { - trainQuery *ecs.Query } -func NewBaliseDetectSystem() *BaliseDetectSystem { - return &BaliseDetectSystem{ - trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)), - } -} +///////////////////////////////////////////////////////////////// -var ( - lastHeadLink string - lastHeadOffset int64 -) - -func (s *BaliseDetectSystem) Update(w ecs.World) { - //所有列车 - //列车速度80KM/H时，222mm/10ms - s.trainQuery.Each(w, func(entry *ecs.Entry) { - s.DetectBalise(w, entry) - }) -} func NewBaliseDetection() *BaliseDetectSystem { return &BaliseDetectSystem{} } // DetectBalise 列车应答器天线探测应答器 -func (s *BaliseDetectSystem) DetectBalise(w ecs.World, trainEntry *ecs.Entry) { +func (s *BaliseDetectSystem) DetectBalise(w ecs.World, trainEntry *ecs.Entry, last *TrainHeadPosition, cur *TrainHeadPosition) { wd := entity.GetWorldData(w) btm := component.TrainBtmType.Get(trainEntry) if btm.PowerAmplifierSwitch { //车载应答器天线功率放大器开启 - tp := component.TrainPositionInfoType.Get(trainEntry) - //测试用 - if tp.HeadLink == lastHeadLink { - dm := math.Abs(float64(tp.HeadLinkOffset-lastHeadOffset) / 1000) - if dm >= 2 { - slog.Debug(fmt.Sprintf("车头移动当前帧与上一帧移动的差值 : %f (m)", dm)) + ranges := s.headMoveRanges(wd, last, cur) + var tbts []*component.TrainBaliseTelegram + aboveBalise := false + for _, r := range ranges { + tt := s.findBalisesInRange(wd, r) + if !aboveBalise { + aboveBalise = len(tt) > 0 + } + for _, detectedBalise := range tt { + //列车应答器天线扫描到应答器,获取应答器报文发送给BTM + tbt := s.findBaliseTelegram(wd, detectedBalise) + if tbt != nil { + tbts = append(tbts, tbt) + } } } - balises := wd.Repo.ResponderListByLink(tp.HeadLink) - detectedBalise := s.detect(wd, tp, balises) - //列车应答器天线扫描到应答器,获取应答器报文发送给BTM - tbt := s.findBaliseTelegram(wd, detectedBalise) - var detectedBaliseId string - if detectedBalise != nil { - detectedBaliseId = detectedBalise.Id() + if len(tbts) <= 0 { + btm.Scanning(aboveBalise, "", nil) + } else { + for _, tbt := range tbts { + btm.Scanning(true, tbt.BaliseId, tbt) + } } - btm.Scanning(detectedBalise != nil, detectedBaliseId, tbt) - //测试用 - lastHeadLink = tp.HeadLink - lastHeadOffset = tp.HeadLinkOffset } } +// 获取车头移动范围内的应答器 +func (s *BaliseDetectSystem) findBalisesInRange(wd *component.WorldData, r *scanRange) []*repository.Transponder { + var tt []*repository.Transponder + balises := wd.Repo.ResponderListByLink(r.linkId) + if r.up { + sort.SliceStable(balises, func(i, j int) bool { + return balises[i].LinkPosition().Offset() < balises[j].LinkPosition().Offset() + }) + } else { + sort.SliceStable(balises, func(i, j int) bool { + return balises[j].LinkPosition().Offset() < balises[i].LinkPosition().Offset() + }) + } + for _, b := range balises { + if r.contains(b.LinkPosition()) { + tt = append(tt, b) + } + } + return tt +} + +// 获取列车车头移动范围 +func (s *BaliseDetectSystem) headMoveRanges(wd *component.WorldData, last *TrainHeadPosition, cur *TrainHeadPosition) []*scanRange { + lastLink := wd.Repo.FindLink(last.HeadLink) + curLink := wd.Repo.FindLink(cur.HeadLink) + var rr []*scanRange + if last.HeadLink == cur.HeadLink { + r := newScanRange(cur.Up, curLink.Id(), last.HeadLinkOffset, cur.HeadLinkOffset) + r.format() + rr = append(rr, r) + } else { + if len(strings.TrimSpace(last.HeadLink)) > 0 { //last 与 cur 不同的道岔连接的轨道 + if last.Up { + r := newScanRange(last.Up, lastLink.Id(), last.HeadLinkOffset, lastLink.Length()) + rr = append(rr, r) + } else { + r := newScanRange(last.Up, lastLink.Id(), 0, last.HeadLinkOffset) + rr = append(rr, r) + } + if cur.Up { + r := newScanRange(cur.Up, curLink.Id(), 0, cur.HeadLinkOffset) + rr = append(rr, r) + } else { + r := newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, curLink.Length()) + rr = append(rr, r) + } + } else { //last不存在 + if cur.Up { //列车运行方向a->b + if cur.HeadLinkOffset >= scanWidth { + return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset-scanWidth, cur.HeadLinkOffset)} + } else { + return []*scanRange{newScanRange(cur.Up, curLink.Id(), 0, cur.HeadLinkOffset)} + } + } else { //列车运行方向b->a + if curLink.Length()-cur.HeadLinkOffset >= scanWidth { + return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, cur.HeadLinkOffset+scanWidth)} + } else { + return []*scanRange{newScanRange(cur.Up, curLink.Id(), cur.HeadLinkOffset, curLink.Length())} + } + } + } + } + // + return rr +} + // 获取应答器激活的有效报文 func (s *BaliseDetectSystem) findBaliseTelegram(wd *component.WorldData, detectedBalise *repository.Transponder) *component.TrainBaliseTelegram { if detectedBalise == nil { @@ -91,52 +141,18 @@ func (s *BaliseDetectSystem) findBaliseTelegram(wd *component.WorldData, detecte // return nil } -func (s *BaliseDetectSystem) detect(wd *component.WorldData, tp *component.TrainPositionInfo, balises []*repository.Transponder) *repository.Transponder { - scanRange := s.calculateScanRange(wd, tp) - if tp.Up { - sort.SliceStable(balises, func(i, j int) bool { - return balises[j].LinkPosition().Offset() < balises[i].LinkPosition().Offset() - }) - } else { - sort.SliceStable(balises, func(i, j int) bool { - return balises[i].LinkPosition().Offset() < balises[j].LinkPosition().Offset() - }) - } - for _, balise := range balises { - if scanRange.contains(balise.LinkPosition()) { - return balise - } - } - return nil -} const scanWidth = int64(1000) //1000mm -// 计算车载应答器天线扫描范围，如果应答器在此范围内则天线可以接收到应答器报文 -func (s *BaliseDetectSystem) calculateScanRange(wd *component.WorldData, tp *component.TrainPositionInfo) *scanRange { - headLink := wd.Repo.FindLink(tp.HeadLink) - if tp.Up { //列车运行方向a->b - if tp.HeadLinkOffset >= scanWidth { - return newScanRange(headLink.Id(), tp.HeadLinkOffset-scanWidth, tp.HeadLinkOffset) - } else { - return newScanRange(headLink.Id(), 0, tp.HeadLinkOffset) - } - } else { //列车运行方向b->a - if headLink.Length()-tp.HeadLinkOffset >= scanWidth { - return newScanRange(headLink.Id(), tp.HeadLinkOffset, tp.HeadLinkOffset+scanWidth) - } else { - return newScanRange(headLink.Id(), tp.HeadLinkOffset, headLink.Length()) - } - } -} type scanRange struct { + up bool //该范围内车的运行方向 linkId string startOffset int64 endOffset int64 } -func newScanRange(linkId string, start int64, end int64) *scanRange { - return &scanRange{linkId: linkId, startOffset: start, endOffset: end} +func newScanRange(up bool, linkId string, start int64, end int64) *scanRange { + return &scanRange{up: up, linkId: linkId, startOffset: start, endOffset: end} } func (s *scanRange) format() { if s.startOffset > s.endOffset { @@ -164,3 +180,12 @@ func (s *scanRange) contains(balisePosition *repository.LinkPosition) bool { s.format() return balisePosition.Offset() >= s.startOffset && balisePosition.Offset() <= s.endOffset } + +type TrainHeadPosition struct { + // 列车运行方向 + Up bool + //列车所在轨道link + HeadLink string + //列车所在link偏移量（mm） + HeadLinkOffset int64 +}