balise detect 预测实现

third_party/can_btm/balise_btm.go

@@ -87,7 +87,7 @@ func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPo
 	//slog.Debug(h.String())
 	wd := entity.GetWorldData(w)
 	repo := wd.Repo
-	s.baliseDetector.Detect(wd, repo, h)
+	s.baliseDetector.detect(wd, repo, h)
 }
 func (s *btmCanetClient) Start(bcm BtmCanetManager) {
 	s.bcm = bcm
@@ -195,7 +195,7 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) {
 	if isResendRequest {
 		s.rspResendToAtp()
 	} else {
-		sb := s.baliseDetector.DoScan()
+		sb := s.baliseDetector.doScan()
 		if sb != nil {
 			slog.Debug(fmt.Sprintf("BTM经过应答器[%s],BTM与ATP时间差[%d]ms", sb.BaliseId, time.Now().UnixMilli()-sb.Time.UnixMilli()))
 			s.rspToAtp(sb)

third_party/can_btm/balise_detection.go

@@ -6,6 +6,7 @@ import (
 	"joylink.club/rtsssimulation/fi"
 	"joylink.club/rtsssimulation/repository"
 	"joylink.club/rtsssimulation/repository/model/proto"
+	"log/slog"
 	"math"
 	"sort"
 	"sync"
@@ -54,9 +55,24 @@ type BaliseDetector struct {
 	baliseCounter int
 	//报文计数器（每解出一个应答器报文加一,应答器报文长度830bits）
 	messageCounter int
+	//BTM所在列车id
+	trianId string
 }
 
-func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
+// 由于同一时间只能有一辆列车与CAN BTM绑定
+// 当检测到重新绑定列车时，重置相关数据
+func (t *BaliseDetector) tryRebind(th *TrainHeadPositionInfo) {
+	if th.TrainId != t.trianId {
+		t.trianId = th.TrainId
+		t.clearExpectedBalise()
+		t.baliseCounter = 0
+		t.messageCounter = 0
+		slog.Debug(fmt.Sprintf("列车[%s]与CAN-BTM绑定", t.trianId))
+	}
+}
+
+func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
+	t.tryRebind(th)
 	if !t.powerAmplifierSwitch { //天线功率放大器未开启，不进行探测
 		return
 	}
@@ -129,7 +145,16 @@ func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseIn
 	t.eq[len(t.eq)-1] = curExpect
 	return true
 }
-func (t *BaliseDetector) DoScan() *BtmAntennaScanningBaliseInfo {
+func (t *BaliseDetector) clearExpectedBalise() {
+	//
+	t.eqLock.Lock()
+	defer t.eqLock.Unlock()
+	//
+	for i := 0; i < len(t.eq); i++ {
+		t.eq[i] = nil
+	}
+}
+func (t *BaliseDetector) doScan() *BtmAntennaScanningBaliseInfo {
 	//
 	t.eqLock.Lock()
 	defer t.eqLock.Unlock()