balise detect 预测实现

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]()
 )

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

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)
+}

sys/device_sys/balise_detection.go

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