From 21ba0216480e01b912068e6fdd57e0f83e687d75 Mon Sep 17 00:00:00 2001
From: xzb <223@qq.com>
Date: Tue, 28 Nov 2023 13:30:04 +0800
Subject: [PATCH] btm

---
 fi/train.go                        |  4 +++
 sys/bind.go                        |  1 -
 sys/device_sys/balise_detection.go | 53 ++++++++++++++++++++++--------
 3 files changed, 43 insertions(+), 15 deletions(-)

diff --git a/fi/train.go b/fi/train.go
index eefce97..c4f16ce 100644
--- a/fi/train.go
+++ b/fi/train.go
@@ -6,6 +6,7 @@ import (
 	"joylink.club/ecs/filter"
 	"joylink.club/rtsssimulation/component"
 	"joylink.club/rtsssimulation/entity"
+	"joylink.club/rtsssimulation/sys/device_sys"
 )
 
 // AddTrainToWorld 添加列车
@@ -48,6 +49,9 @@ func UpdateTrainPositionFromDynamics(w ecs.World, tpi TrainPositionInfo) error {
 			train.HeadLinkOffset = int64(tpi.HeadLinkOffset)
 			train.TailLink = tpi.TailLink
 			train.TailLinkOffset = int64(tpi.TailLinkOffset)
+			//根据列车位置探测应答器
+			device_sys.NewBaliseDetection().DetectBalise(w, te)
+			//
 			return ecs.NewOkEmptyResult()
 		} else {
 			return ecs.NewErrResult(fmt.Errorf("列车[%s]实体不存在", tpi.TrainId))
diff --git a/sys/bind.go b/sys/bind.go
index fa2dcde..e88a83e 100644
--- a/sys/bind.go
+++ b/sys/bind.go
@@ -39,6 +39,5 @@ func BindSystem(w ecs.World) {
 		device_sys.NewSectionDetectSystem(),
 		//应答器
 		device_sys.NewBaliseSystem(),
-		device_sys.NewBaliseDetectSystem(),
 	)
 }
diff --git a/sys/device_sys/balise_detection.go b/sys/device_sys/balise_detection.go
index 0c698db..04838e4 100644
--- a/sys/device_sys/balise_detection.go
+++ b/sys/device_sys/balise_detection.go
@@ -8,6 +8,7 @@ import (
 	"joylink.club/rtsssimulation/entity"
 	"joylink.club/rtsssimulation/repository"
 	"log/slog"
+	"math"
 	"sort"
 	"strings"
 )
@@ -24,26 +25,50 @@ func NewBaliseDetectSystem() *BaliseDetectSystem {
 		trainQuery: ecs.NewQuery(filter.Contains(component.UidType, component.TrainPositionInfoType, component.TrainBtmType)),
 	}
 }
+
+var (
+	lastHeadLink   string
+	lastHeadOffset int64
+)
+
 func (s *BaliseDetectSystem) Update(w ecs.World) {
-	wd := entity.GetWorldData(w)
 	//所有列车
 	//列车速度80KM/H时，222mm/10ms
 	s.trainQuery.Each(w, func(entry *ecs.Entry) {
-		btm := component.TrainBtmType.Get(entry)
-		if btm.PowerAmplifierSwitch { //车载应答器天线功率放大器开启
-			tp := component.TrainPositionInfoType.Get(entry)
-			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()
-			}
-			btm.Scanning(detectedBalise != nil, detectedBaliseId, tbt)
-		}
+		s.DetectBalise(w, entry)
 	})
 }
+func NewBaliseDetection() *BaliseDetectSystem {
+	return &BaliseDetectSystem{}
+}
+
+// DetectBalise 列车应答器天线探测应答器
+func (s *BaliseDetectSystem) DetectBalise(w ecs.World, trainEntry *ecs.Entry) {
+	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))
+			}
+		}
+		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()
+		}
+		btm.Scanning(detectedBalise != nil, detectedBaliseId, tbt)
+		//测试用
+		lastHeadLink = tp.HeadLink
+		lastHeadOffset = tp.HeadLinkOffset
+	}
+}
 
 // 获取应答器激活的有效报文
 func (s *BaliseDetectSystem) findBaliseTelegram(wd *component.WorldData, detectedBalise *repository.Transponder) *component.TrainBaliseTelegram {