joylink/rts-sim-module
6
0
Fork 0
Code Issues 1 Pull Requests Packages Projects Releases Wiki Activity

btm

Browse Source
This commit is contained in:
xzb 2023-11-28 16:05:41 +08:00
parent 21ba021648
commit ed536bd92d
3 changed files with 106 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
component/train.go
View File

@ -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

7
fi/train.go
View File

@ -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 {

175
sys/device_sys/balise_detection.go
View File

@ -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
}