Merge branch 'master' of https://git.code.tencent.com/beijing-rtss-test/bj-rtsts-server-go

third_party/can_btm/balise_btm.go

@@ -5,8 +5,8 @@ import (
 	"joylink.club/bj-rtsts-server/config"
 	"joylink.club/bj-rtsts-server/third_party/message"
 	"joylink.club/bj-rtsts-server/third_party/udp"
-	"joylink.club/bj-rtsts-server/ts/simulation/wayside/memory"
-	"joylink.club/rtsssimulation/fi"
+	"joylink.club/ecs"
+	"joylink.club/rtsssimulation/entity"
 	"log/slog"
 	"sort"
 	"sync"
@@ -17,7 +17,7 @@ import (
 
 // BtmCanetManager BTM CANET 管理器
 type BtmCanetManager interface {
-	memory.VerifyEvn
+	EvnWorld() ecs.World
 	//GetBtmCanetConfig 获取CANET配置信息
 	GetBtmCanetConfig() config.BtmCanetConfig
 }
@@ -48,6 +48,8 @@ type btmCanetClient struct {
 	dsn byte
 	//重发的数据
 	resendData *resendData
+	//车载BTM天线，探测应答器
+	baliseDetector *BaliseDetector
 }
 type btmClock struct {
 	btmTk uint32    //与ATP系统同步的时间ms
@@ -62,6 +64,8 @@ func (c *btmClock) tkNow() uint32 {
 type BtmCanetClient interface {
 	Start(bcm BtmCanetManager)
 	Stop()
+	//HandleTrainHeadPositionInfo 处理收到列车位置信息
+	HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo)
 }
 
 var (
@@ -73,11 +77,18 @@ func Default() BtmCanetClient {
 	btmClientLocker.Lock()
 	defer btmClientLocker.Unlock()
 	if btmClient == nil {
-		btmClient = &btmCanetClient{}
+		btmClient = &btmCanetClient{baliseDetector: &BaliseDetector{}}
 	}
 	return btmClient
 }
 
+// HandleTrainHeadPositionInfo 处理来自动力学的列车位置信息
+func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo) {
+	//slog.Debug(h.String())
+	wd := entity.GetWorldData(w)
+	repo := wd.Repo
+	s.baliseDetector.detect(wd, repo, h)
+}
 func (s *btmCanetClient) Start(bcm BtmCanetManager) {
 	s.bcm = bcm
 	cfg := s.bcm.GetBtmCanetConfig()
@@ -176,36 +187,55 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) {
 	s.btmTime.sysTk = time.Now()
 	s.atpReqSn = atpReq.FId.ID4
 	s.atpReqCrc16Check = atpReq.Crc16CheckOk
-	se := fi.TrainBalisePowerAmplifierSwitch(s.bcm.EvnWorld(), atpReq.PowerAmplifierTurnOn)
-	if se != nil {
-		//slog.Warn(fmt.Sprintf("列车车载BTM功率放大器开关控制异常[%s]", se.Error()))
-	}
-	//ATP 是否要求BTM 重发上一应答器报文
-	isResendRequest := atpReq.ResendRequest == 2 //0b10
-	s.rspToAtp(isResendRequest)
+	s.baliseDetector.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn
 	//记录atp查询时间
 	s.atpReqTime = &now
+	//ATP 是否要求BTM 重发上一应答器报文
+	isResendRequest := atpReq.ResendRequest == 2 //0b10
+	if isResendRequest {
+		s.rspResendToAtp()
+	} else {
+		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)
+		} else {
+			s.rspToAtp(nil)
+		}
+	}
+}
+
+func (s *btmCanetClient) createTrainBtmStatus() *TrainBtmStatus {
+	return &TrainBtmStatus{
+		PowerAmplifierOn:    s.baliseDetector.powerAmplifierSwitch,
+		PowerAmplifierFault: false,
+		AboveBalise:         s.baliseDetector.aboveBalise,
+		AntennaFault:        false,
+		BaliseCounter:       s.baliseDetector.baliseCounter,
+		MessageCounter:      s.baliseDetector.messageCounter,
+	}
 }
 
 // BTM发送响应给ATP
 // 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
 // 当未收到应答器报文时响应：时间同步帧、状态应答帧
-func (s *btmCanetClient) rspToAtp(isResendRequest bool) {
+func (s *btmCanetClient) rspResendToAtp() {
 	//重发上一报文处理
-	if isResendRequest && s.resendData != nil && s.resendData.canResend() {
+	if s.resendData != nil && s.resendData.canResend() {
 		s.resendData.countAdd1()
 		s.sendCanetFrame(s.resendData.data)
 		return
-	} else {
-		s.resendData = nil
 	}
+}
+
+// BTM发送响应给ATP
+// 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
+// 当未收到应答器报文时响应：时间同步帧、状态应答帧
+func (s *btmCanetClient) rspToAtp(sb *BtmAntennaScanningBaliseInfo) {
 	//BTM状态
 	statusF := message.NewBtmStatusRspFrame(s.atpReqSn)
-	btmStatus, btmStatusErr := fi.FindTrainBaliseBtmStatus(s.bcm.EvnWorld())
-	if btmStatusErr != nil {
-		//slog.Debug(fmt.Sprintf("从仿真获取BTM状态失败：%s", btmStatusErr.Error()))
-		return
-	}
+	btmStatus := s.createTrainBtmStatus()
+
 	statusF.AntennaFault = btmStatus.AntennaFault
 	statusF.BaliseCounter = byte(btmStatus.BaliseCounter)
 	statusF.MessageCounter = byte(btmStatus.MessageCounter)
@@ -220,16 +250,10 @@ func (s *btmCanetClient) rspToAtp(isResendRequest bool) {
 	statusF.Dsn = s.dsn
 	s.dsnAdd1()
 	//
-	baliseTelegram, btSendErr := fi.GetScannedBaliseTelegram(s.bcm.EvnWorld())
-	if btSendErr != nil {
-		//slog.Debug(btSendErr.Error())
-	} else {
-		slog.Debug(fmt.Sprintf("发送应答器报文，应答器：[%s]", baliseTelegram.BaliseId))
-	}
 	//true-收到应答器报文
-	isRcvTelegram := baliseTelegram != nil
+	isRcvTelegram := sb != nil && len(sb.telegram) > 0
 	if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
-		statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram.Telegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow())
+		statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, sb.telegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow())
 		if statusDataCfOk {
 			timeSyncF := message.NewBtmTimeSyncCheckFrame(s.atpReqSn)
 			timeSyncF.T2 = s.btmTime.btmTk
@@ -301,3 +325,14 @@ func (s *btmCanetClient) dealWithBtmDataFrames(dms []*message.CanetFrame) {
 		slog.Debug(fmt.Sprintf("接收到数据帧%s", dm.String()))
 	}
 }
+
+//////////////////////////////////////////////////////////////////////
+
+type TrainBtmStatus struct {
+	PowerAmplifierOn    bool //BTM功率放大器是否开启
+	PowerAmplifierFault bool //BTM功率放大器是否有故障
+	AntennaFault        bool //BTM应答器天线是否有故障
+	AboveBalise         bool //BTM当前是否在应答器上方
+	BaliseCounter       int  //应答器计数（每过一个应答器加一，在同一个应答器内不变）
+	MessageCounter      int  //报文计数器（每解出一个应答器报文加一）
+}

third_party/can_btm/balise_detection.go

@@ -3,6 +3,7 @@ package can_btm
 import (
 	"fmt"
 	"joylink.club/rtsssimulation/component"
+	"joylink.club/rtsssimulation/fi"
 	"joylink.club/rtsssimulation/repository"
 	"joylink.club/rtsssimulation/repository/model/proto"
 	"log/slog"
@@ -13,6 +14,7 @@ import (
 )
 
 //通过提前预测来实现BTM天线扫描应答器的实时性
+//根据当前列车运行信息预测出列车前方应答器被扫描到的时刻
 
 // BtmAntennaRunningInfo 车载BTM天线中心点位置运行信息
 // BTM天线一般在第一车轴后某个位置
@@ -29,20 +31,7 @@ const (
 )
 
 // 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
-}
+type TrainHeadPositionInfo = fi.TrainHeadPositionInfo
 type BtmAntennaToBaliseInfo struct {
 	Distance int64  //BTM天线中心到应答器的距离，mm
 	BaliseId string //应答器id
@@ -50,43 +39,137 @@ type BtmAntennaToBaliseInfo struct {
 type BtmAntennaScanningBaliseInfo struct {
 	BaliseId string    //应答器id
 	Time     time.Time //应答器预计被BTM天线激活的时刻
-}
-type BaliseDetector struct {
-	basbi     *BtmAntennaScanningBaliseInfo
-	basbiLock sync.Mutex
+	active   bool      //true-激活过，即列车扫过
+	telegram []byte    //应答器报文
 }
 
-func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
+// BaliseDetector 车载BTM天线，应答器探测器
+type BaliseDetector struct {
+	eq     [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头
+	eqLock sync.Mutex
+	//车载应答器天线功率放大器开关，true-开，false-关
+	powerAmplifierSwitch bool
+	//天线此时是否在应答器上方
+	aboveBalise bool
+	//应答器计数（每过一个应答器加一，在同一个应答器内不变）
+	baliseCounter int
+	//报文计数器（每解出一个应答器报文加一,应答器报文长度830bits）
+	messageCounter int
+	//BTM所在列车id
+	trianId string
+}
+
+// 由于同一时间只能有一辆列车与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
+	}
 	curTime := time.Now()
-	bari := t.createBtmAntennaRunningInfo(wd, repo, th)
+	//BTM天线中心点运行信息
+	curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)
 	//预测BTM天线到最近一个应答器的时刻
-	basbi := t.timeScanNearestBalise(curTime, wd, repo, bari)
-	if basbi != nil {
-		t.updateBasbi(basbi)
-		slog.Debug("将要激活应答器", "BaliseId", basbi.BaliseId, "ActiveTime", basbi.Time)
-	}
-}
-func (t *BaliseDetector) ScanBaliseByTime(curTime time.Time) *BtmAntennaScanningBaliseInfo {
-	t.basbiLock.Lock()
-	defer t.basbiLock.Unlock()
-	//
-	if t.basbi != nil && curTime.UnixMilli() >= t.basbi.Time.UnixMilli() {
-		rt := *t.basbi
-		return &rt
-	}
-	return nil
-}
-func (t *BaliseDetector) updateBasbi(basbi *BtmAntennaScanningBaliseInfo) {
-	t.basbiLock.Lock()
-	defer t.basbiLock.Unlock()
-	//
-	if basbi != nil {
-		*t.basbi = *basbi
+	curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)
+	if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms
+		//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
+		//记录即将经过的应答器
+		if t.addExpectedBalise(curExpect) {
+			t.baliseCounterAdd1() //应答器计数器
+			telegram := t.rcvTelegram(wd, curExpect.BaliseId)
+			if len(telegram) > 0 {
+				curExpect.telegram = telegram
+				t.baliseMessageCounterAdd1() //报文计数器
+			}
+		}
+		//BTM天线即将经过应答器
+		t.aboveBalise = true
 	} else {
-		t.basbi = nil
+		t.aboveBalise = false
 	}
 }
 
+// 应答器计数器加1，[0,255]
+func (t *BaliseDetector) baliseCounterAdd1() {
+	t.baliseCounter++
+	if t.baliseCounter > 255 {
+		t.baliseCounter = 0
+	}
+}
+
+// 报文计数器加1，[0,255]
+func (t *BaliseDetector) baliseMessageCounterAdd1() {
+	t.messageCounter++
+	if t.messageCounter > 255 {
+		t.messageCounter = 0
+	}
+}
+
+// BTM天线接收应答器报文(线程不安全)
+func (t *BaliseDetector) rcvTelegram(wd *component.WorldData, baliseId string) []byte {
+	entry, ok := wd.EntityMap[baliseId]
+	if !ok {
+		return nil
+	} else {
+		return component.BaliseStateType.Get(entry).ValidTelegram
+	}
+}
+
+// true-新增；false-更新
+func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) bool {
+	//
+	t.eqLock.Lock()
+	defer t.eqLock.Unlock()
+	//更新,去重
+	for i, e := range t.eq {
+		if e != nil && e.BaliseId == curExpect.BaliseId {
+			t.eq[i].Time = curExpect.Time
+			return false
+		}
+	}
+	//左移
+	for i := 1; i < len(t.eq); i++ {
+		t.eq[i-1] = t.eq[i]
+	}
+	//存入队尾
+	t.eq[len(t.eq)-1] = curExpect
+	return true
+}
+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()
+	//
+	var rt *BtmAntennaScanningBaliseInfo
+	for i := 0; i < len(t.eq); i++ {
+		if t.eq[i] != nil && !t.eq[i].active {
+			rt = t.eq[i]
+			t.eq[i].active = true
+			break
+		}
+	}
+	return rt
+}
+
 // 计算列车在当前运行状态下，预测到最近一个应答器的时刻
 func (t *BaliseDetector) timeScanNearestBalise(curTime time.Time, wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaScanningBaliseInfo {
 	expectedBalise := t.findBaliseWillScanByBtmAntenna(wd, repo, ba)
@@ -225,6 +308,10 @@ func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.R
 	curLink := repo.FindLink(curLinkId)
 	if searchDirection {
 		bDc := curLink.BRelation()
+		if bDc == nil || bDc.Turnout() == nil {
+			return nil
+		}
+		//
 		bDcDw := t.turnoutPosition(wd, bDc.Turnout().Id())
 		switch bDc.Port() {
 		case proto.Port_A: //link-b连接turnout-A
@@ -240,6 +327,10 @@ func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.R
 		}
 	} else {
 		aDc := curLink.ARelation()
+		if aDc == nil || aDc.Turnout() == nil {
+			return nil
+		}
+		//
 		aDcDw := t.turnoutPosition(wd, aDc.Turnout().Id())
 		switch aDc.Port() {
 		case proto.Port_A: //link-a连接turnout-A
@@ -283,7 +374,8 @@ func (t *BaliseDetector) calculateQuadratic(a, b, c float64) (float64, bool) {
 		x1 := (-b + sqrtD) / ax2
 		x2 := (-b - sqrtD) / ax2
 		if x1 >= 0 && x2 >= 0 {
-			panic("x1 >= 0 && x2 >= 0")
+			//panic("x1 >= 0 && x2 >= 0")
+			return 0, false
 		}
 		if x1 >= 0 {
 			return x1, true

ts/simulation/wayside/memory/wayside_memory_train.go

@@ -2,6 +2,7 @@ package memory
 
 import (
 	"fmt"
+	"joylink.club/bj-rtsts-server/third_party/can_btm"
 	"log/slog"
 	"math"
 	"strconv"
@@ -126,6 +127,15 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
 		panic(sys_error.New("动力学传输数据:列车车尾位置计算出错", e2))
 	}
 	//slog.Debug("车尾位置", tailDeviceId, "偏移", tailDeviceOffset, "所在设备端", tailDevicePort)
+	// 更新BTM中列车位置信息
+	can_btm.Default().HandleTrainHeadPositionInfo(vs.World, &fi.TrainHeadPositionInfo{
+		TrainId:      trainId,
+		Up:           info.Up,
+		Link:         outLinkId,
+		LinkOffset:   outLinkOffset,
+		Speed:        info.Speed,
+		Acceleration: info.Acceleration,
+	})
 	// 修改world中的列车位置
 	fi.UpdateTrainPositionFromDynamics(vs.World, fi.TrainPositionInfo{
 		TrainId:        trainId,