diff --git a/component/train.go b/component/train.go index d4f49b1..b3a91c2 100644 --- a/component/train.go +++ b/component/train.go @@ -26,115 +26,6 @@ func (t *TrainPositionInfo) ToString() string { return fmt.Sprintf("Up=%t len=%d headLink=%s headOff=%d tailLink=%s tailOff=%d", t.Up, t.Len, t.HeadLink, t.HeadLinkOffset, t.TailLink, t.TailLinkOffset) } -// TrainBaliseTelegram 应答器报文 -type TrainBaliseTelegram struct { - BaliseId string //应答器ID - Telegram []byte //一个应答器同一时刻只有一条报文处于激活有效状态 - Up bool //列车经过应答器时的运行方向 -} - -func NewTrainBaliseTelegram(up bool, baliseId string, telegram []byte) *TrainBaliseTelegram { - return &TrainBaliseTelegram{ - BaliseId: baliseId, - Telegram: telegram, - Up: up, - } -} - -// TrainBtm 列车应答器传输模块 -type TrainBtm struct { - //应答器计数(每过一个应答器加一,在同一个应答器内不变) - BaliseCounter int - //报文计数器(每解出一个应答器报文加一) - MessageCounter int - //车载应答器天线功率放大器开关,true-开,false-关 - PowerAmplifierSwitch bool - //天线此时是否在应答器上方 - AboveBalise bool - //列车在运行方向顺序扫描到的应答器 - //扫描到的应答器报文缓存区 - //当BTM超过150ms即3个周期没有收到atq下发的查询帧,则清空报文缓冲区 - scannedBalises *telegramQueue - //最近经过的应答器id - lastTelegram *TrainBaliseTelegram -} - -func NewTrainBtm() *TrainBtm { - return &TrainBtm{scannedBalises: &telegramQueue{}} -} - -// 应答器计数器加1,[0,255] -func (t *TrainBtm) baliseCounterAdd1() { - t.BaliseCounter++ - if t.BaliseCounter > 255 { - t.BaliseCounter = 0 - } -} - -// 报文计数器加1,[0,255] -func (t *TrainBtm) baliseMessageCounterAdd1() { - t.MessageCounter++ - if t.MessageCounter > 255 { - t.MessageCounter = 0 - } -} - -func (t *TrainBtm) FindScannedBalises() *TrainBaliseTelegram { - return t.scannedBalises.removeHead() -} -func (t *TrainBtm) ClearScannedBalises() { - t.scannedBalises.clear() -} - -// Scanning BTM通过车载应答器天线接收到应答器报文 -func (t *TrainBtm) Scanning(aboveBalise bool, telegram *TrainBaliseTelegram) { - //新报文 - isNewTbt := telegram != nil && (t.lastTelegram == nil || t.lastTelegram.Up != telegram.Up || t.lastTelegram.BaliseId != telegram.BaliseId) - if isNewTbt { - t.lastTelegram = telegram - t.scannedBalises.addTail(telegram) - t.baliseMessageCounterAdd1() - } - // - t.AboveBalise = aboveBalise - //BTM此时在一个新的应答器上方 - if t.AboveBalise || isNewTbt { - t.baliseCounterAdd1() - //slog.Debug(fmt.Sprintf("列车经过应答器上方,应答器:[%s]", t.viaBaliseId)) - } -} - -type telegramQueue struct { - queue [3]*TrainBaliseTelegram -} - -func (q *telegramQueue) moveHead() { - q.queue[0], q.queue[1], q.queue[2] = q.queue[1], q.queue[2], nil -} -func (q *telegramQueue) addTail(t *TrainBaliseTelegram) { - if q.queue[len(q.queue)-1] == nil { - q.queue[len(q.queue)-1] = t - } else { - q.moveHead() - q.queue[len(q.queue)-1] = t - } -} -func (q *telegramQueue) removeHead() *TrainBaliseTelegram { - for i, rt := range q.queue { - if rt != nil { - q.queue[i] = nil - return rt - } - } - return nil -} -func (q *telegramQueue) clear() { - for i, _ := range q.queue { - q.queue[i] = nil - } -} - var ( TrainPositionInfoType = ecs.NewComponentType[TrainPositionInfo]() - TrainBtmType = ecs.NewComponentType[TrainBtm]() ) diff --git a/entity/train.go b/entity/train.go index 81f4849..dd974a5 100644 --- a/entity/train.go +++ b/entity/train.go @@ -13,12 +13,3 @@ func NewTrainEntity(w ecs.World, trainId string) *ecs.Entry { data.EntityMap[trainId] = te return te } -func NewTrainWithBtmEntity(w ecs.World, trainId string) *ecs.Entry { - data := GetWorldData(w) - te := w.Entry(w.Create(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)) - component.UidType.SetValue(te, component.Uid{Id: trainId}) - component.TrainPositionInfoType.Set(te, &component.TrainPositionInfo{}) - component.TrainBtmType.Set(te, component.NewTrainBtm()) - data.EntityMap[trainId] = te - return te -} diff --git a/fi/train.go b/fi/train.go index 7d79b06..6081487 100644 --- a/fi/train.go +++ b/fi/train.go @@ -3,10 +3,8 @@ package fi import ( "fmt" "joylink.club/ecs" - "joylink.club/ecs/filter" "joylink.club/rtsssimulation/component" "joylink.club/rtsssimulation/entity" - "joylink.club/rtsssimulation/sys/device_sys" ) // AddTrainToWorld 添加列车 @@ -15,7 +13,7 @@ func AddTrainToWorld(w ecs.World, trainId string) error { wd := entity.GetWorldData(w) _, find := wd.EntityMap[trainId] if !find { - entity.NewTrainWithBtmEntity(w, trainId) + entity.NewTrainEntity(w, trainId) } return ecs.NewOkEmptyResult() }) @@ -44,7 +42,7 @@ func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error { if find { train := component.TrainPositionInfoType.Get(te) // - lastTrainHeadPos := &device_sys.TrainHeadPosition{Up: train.Up, HeadLink: train.HeadLink, HeadLinkOffset: train.HeadLinkOffset} + //lastTrainHeadPos := &device_sys.TrainHeadPosition{Up: train.Up, HeadLink: train.HeadLink, HeadLinkOffset: train.HeadLinkOffset} // train.Up = tpi.Up train.Len = int64(tpi.Len) @@ -52,10 +50,12 @@ func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error { 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, lastTrainHeadPos, curTrainHeadPos) + /* + //列车车头移动范围 + curTrainHeadPos := &device_sys.TrainHeadPosition{Up: train.Up, HeadLink: train.HeadLink, HeadLinkOffset: train.HeadLinkOffset} + //根据列车位置探测应答器 + device_sys.NewBaliseDetection().DetectBalise(w, te, lastTrainHeadPos, curTrainHeadPos) + */ // return ecs.NewOkEmptyResult() } else { @@ -66,92 +66,8 @@ func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error { return result.Err } -// FindTrainBaliseBtmStatus 获取车载BTM的相关状态信息 -func FindTrainBaliseBtmStatus(w ecs.World) (*TrainBtmStatus, error) { - result := <-ecs.Request[*TrainBtmStatus](w, func() ecs.Result[*TrainBtmStatus] { - //获取当前关联CANET设备的列车 - canetTrain := findCanetTrain(w) - if canetTrain == nil { - return ecs.NewResult[*TrainBtmStatus](nil, fmt.Errorf("系统中当前不存在与CANET设备关联的列车实体")) - } - // - trainId := component.UidType.Get(canetTrain).Id - btm := component.TrainBtmType.Get(canetTrain) - btmStatus := &TrainBtmStatus{ - TrainId: trainId, - PowerAmplifierOn: btm.PowerAmplifierSwitch, - PowerAmplifierFault: false, - AboveBalise: btm.AboveBalise, - AntennaFault: false, - BaliseCounter: btm.BaliseCounter, - MessageCounter: btm.MessageCounter, - } - return ecs.NewOkResult(btmStatus) - }) - return result.Val, result.Err -} - -// TrainBalisePowerAmplifierSwitch 车载应答器天线功率放大器开关控制 -func TrainBalisePowerAmplifierSwitch(w ecs.World, turnOn bool) error { - result := <-ecs.Request[ecs.EmptyType](w, func() ecs.Result[ecs.EmptyType] { - //获取当前关联CANET设备的列车 - canetTrain := findCanetTrain(w) - if canetTrain == nil { - return ecs.NewErrResult(fmt.Errorf("系统中当前不存在与CANET设备关联的列车实体")) - } - // - train := component.TrainBtmType.Get(canetTrain) - train.PowerAmplifierSwitch = turnOn - return ecs.NewOkEmptyResult() - }) - return result.Err -} - -// GetScannedBaliseTelegram 获取扫描到的应答器报文 -func GetScannedBaliseTelegram(w ecs.World) (*component.TrainBaliseTelegram, error) { - result := <-ecs.Request[*component.TrainBaliseTelegram](w, func() ecs.Result[*component.TrainBaliseTelegram] { - //获取当前关联CANET设备的列车 - canetTrain := findCanetTrain(w) - if canetTrain == nil { - return ecs.NewResult[*component.TrainBaliseTelegram](nil, fmt.Errorf("系统中当前不存在与CANET设备关联的列车实体")) - } - // - train := component.TrainBtmType.Get(canetTrain) - tbt := train.FindScannedBalises() - if tbt != nil { - return ecs.NewOkResult(tbt) - } else { - return ecs.NewResult[*component.TrainBaliseTelegram](nil, fmt.Errorf("此刻没有要发送的应答器报文")) - } - }) - return result.Val, result.Err -} - -var trainQuery = ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)) - -// 获取当前关联CANET设备的列车 -func findCanetTrain(w ecs.World) *ecs.Entry { - var canetTrain *ecs.Entry = nil - trainQuery.Each(w, func(entry *ecs.Entry) { //目前默认取第一辆 - if canetTrain == nil { - canetTrain = entry - } - }) - return canetTrain -} - // ///////////////////////////////////////////////////////////////////////////////////////// -type TrainBtmStatus struct { - TrainId string - PowerAmplifierOn bool //BTM功率放大器是否开启 - PowerAmplifierFault bool //BTM功率放大器是否有故障 - AntennaFault bool //BTM应答器天线是否有故障 - AboveBalise bool //BTM当前是否在应答器上方 - BaliseCounter int //应答器计数(每过一个应答器加一,在同一个应答器内不变) - MessageCounter int //报文计数器(每解出一个应答器报文加一) -} - type TrainPositionInfo struct { //列车id TrainId string @@ -168,3 +84,24 @@ type TrainPositionInfo struct { //列车所在link偏移量(mm) TailLinkOffset uint32 } + +// 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 +} + +func (t *TrainHeadPositionInfo) String() string { + return fmt.Sprintf("TrainHeadPositionInfo :: TrainId=%s Up=%t Link=%s LinkOffset=%d Speed=%f Ac=%f", + t.TrainId, t.Up, t.Link, t.LinkOffset, t.Speed, t.Acceleration) +} diff --git a/sys/device_sys/balise_detection.go b/sys/device_sys/balise_detection.go deleted file mode 100644 index d76a84d..0000000 --- a/sys/device_sys/balise_detection.go +++ /dev/null @@ -1,193 +0,0 @@ -package device_sys - -import ( - "fmt" - "joylink.club/ecs" - "joylink.club/rtsssimulation/component" - "joylink.club/rtsssimulation/entity" - "joylink.club/rtsssimulation/repository" - "log/slog" - "sort" - "strings" -) - -// BaliseDetectSystem 列车应答器天线探测轨旁应答器 -// 一条应答器报文长830bits;单个应答器对应多条报文; -// 固定应答器对应1条报文;默认报文对应1条; -type BaliseDetectSystem struct { -} - -///////////////////////////////////////////////////////////////// - -func NewBaliseDetection() *BaliseDetectSystem { - return &BaliseDetectSystem{} -} - -// DetectBalise 列车应答器天线探测应答器 -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 { //车载应答器天线功率放大器开启 - 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, r.up, detectedBalise) - if tbt != nil { - tbts = append(tbts, tbt) - } - } - } - if len(tbts) <= 0 { - btm.Scanning(aboveBalise, nil) - } else { - for _, tbt := range tbts { - btm.Scanning(true, tbt) - } - } - } else { - btm.ClearScannedBalises() - } -} - -// 获取车头移动范围内的应答器 -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, up bool, detectedBalise *repository.Transponder) *component.TrainBaliseTelegram { - if detectedBalise == nil { - return nil - } - // - baliseEntry := wd.EntityMap[detectedBalise.Id()] - if baliseEntry != nil { - baliseState := component.BaliseStateType.Get(baliseEntry) - //测试,设置应答器默认报文 - baliseState.ValidTelegram = []byte{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12} - // - if len(baliseState.ValidTelegram) > 0 { - return component.NewTrainBaliseTelegram(up, detectedBalise.Id(), baliseState.ValidTelegram) - } - } else { - slog.Warn(fmt.Sprintf("应答器[%s]实体不存在", detectedBalise.Id())) - } - // - return nil -} - -const scanWidth = int64(1000) //1000mm - -type scanRange struct { - up bool //该范围内车的运行方向 - linkId string - startOffset int64 - endOffset int64 -} - -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 { - s.startOffset, s.endOffset = s.endOffset, s.startOffset - } -} -func (s *scanRange) str() string { - sb := strings.Builder{} - sb.WriteString("LinkRange[") - sb.WriteString("linkId:") - sb.WriteString(s.linkId) - sb.WriteString(" start:") - sb.WriteString(fmt.Sprintf("%d", s.startOffset)) - sb.WriteString(" end:") - sb.WriteString(fmt.Sprintf("%d", s.endOffset)) - sb.WriteString("]") - return sb.String() -} - -// true-应答器balisePosition在该scanRange内 -func (s *scanRange) contains(balisePosition *repository.LinkPosition) bool { - if s.linkId != balisePosition.Link().Id() { - return false - } - s.format() - return balisePosition.Offset() >= s.startOffset && balisePosition.Offset() <= s.endOffset -} - -type TrainHeadPosition struct { - // 列车运行方向 - Up bool - //列车所在轨道link - HeadLink string - //列车所在link偏移量(mm) - HeadLinkOffset int64 -}