joylink/rts-sim-testing-service
6
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
weizhihong 2023-12-05 13:59:34 +08:00
commit 745fb55110
3 changed files with 209 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

91
third_party/can_btm/balise_btm.go vendored
View File

@ -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 //报文计数器（每解出一个应答器报文加一）
}

180
third_party/can_btm/balise_detection.go vendored
View File

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

10
ts/simulation/wayside/memory/wayside_memory_train.go
View File

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