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

balise detect 预测实现

This commit is contained in:
xzb 2023-12-05 09:37:34 +08:00
parent a499ec1ba9
commit 163c0f53c2
3 changed files with 178 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

123
third_party/can_btm/balise_btm.go vendored
View File

@ -5,8 +5,9 @@ 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/component"
"joylink.club/rtsssimulation/entity"
"log/slog"
"sort"
"sync"
@ -17,7 +18,7 @@ import (
// BtmCanetManager BTM CANET 管理器
type BtmCanetManager interface {
memory.VerifyEvn
EvnWorld() ecs.World
//GetBtmCanetConfig 获取CANET配置信息
GetBtmCanetConfig() config.BtmCanetConfig
}
@ -48,6 +49,16 @@ type btmCanetClient struct {
dsn byte
//重发的数据
resendData *resendData
//车载BTM天线，探测应答器
baliseDetector *BaliseDetector
//车载应答器天线功率放大器开关，true-开，false-关
powerAmplifierSwitch bool
//天线此时是否在应答器上方
aboveBalise bool
//应答器计数（每过一个应答器加一，在同一个应答器内不变）
baliseCounter int
//报文计数器（每解出一个应答器报文加一）
messageCounter int
}
type btmClock struct {
btmTk uint32 //与ATP系统同步的时间ms
@ -62,6 +73,8 @@ func (c *btmClock) tkNow() uint32 {
type BtmCanetClient interface {
Start(bcm BtmCanetManager)
Stop()
//HandleTrainHeadPositionInfo 处理收到列车位置信息
HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo)
}
var (
@ -73,11 +86,32 @@ func Default() BtmCanetClient {
btmClientLocker.Lock()
defer btmClientLocker.Unlock()
if btmClient == nil {
btmClient = &btmCanetClient{}
btmClient = &btmCanetClient{baliseDetector: &BaliseDetector{}}
}
return btmClient
}
// 应答器计数器加1，[0,255]
func (t *btmCanetClient) baliseCounterAdd1() {
t.baliseCounter++
if t.baliseCounter > 255 {
t.baliseCounter = 0
}
}
// 报文计数器加1，[0,255]
func (t *btmCanetClient) baliseMessageCounterAdd1() {
t.messageCounter++
if t.messageCounter > 255 {
t.messageCounter = 0
}
}
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 +210,65 @@ 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.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn
//记录atp查询时间
s.atpReqTime = &now
//ATP 是否要求BTM 重发上一应答器报文
isResendRequest := atpReq.ResendRequest == 2 //0b10
s.aboveBalise = s.baliseDetector.HasBaliseBeingScan()
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)
}
}
}
func (s *btmCanetClient) createTrainBtmStatus() *TrainBtmStatus {
return &TrainBtmStatus{
PowerAmplifierOn: s.powerAmplifierSwitch,
PowerAmplifierFault: false,
AboveBalise: s.aboveBalise,
AntennaFault: false,
BaliseCounter: s.baliseCounter,
MessageCounter: s.messageCounter,
}
}
// 获取应答器报文(线程不安全)
func (s *btmCanetClient) findBaliseTelegram(baliseId string) []byte {
wd := entity.GetWorldData(s.bcm.EvnWorld())
entry, ok := wd.EntityMap[baliseId]
if !ok {
return nil
} else {
return component.BaliseStateType.Get(entry).ValidTelegram
}
}
// 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 +283,11 @@ 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))
}
baliseTelegram := s.findBaliseTelegram(sb.BaliseId)
//true-收到应答器报文
isRcvTelegram := baliseTelegram != nil
if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram.Telegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow())
statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram, false, s.btmTime.tkNow(), s.btmTime.tkNow(), s.btmTime.tkNow())
if statusDataCfOk {
timeSyncF := message.NewBtmTimeSyncCheckFrame(s.atpReqSn)
timeSyncF.T2 = s.btmTime.btmTk
@ -301,3 +359,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 //报文计数器（每解出一个应答器报文加一）
}

110
third_party/can_btm/balise_detection.go vendored
View File

@ -3,9 +3,9 @@ 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"
"math"
"sort"
"sync"
@ -13,6 +13,7 @@ import (
)
//通过提前预测来实现BTM天线扫描应答器的实时性
//根据当前列车运行信息预测出列车前方应答器被扫描到的时刻
// BtmAntennaRunningInfo 车载BTM天线中心点位置运行信息
// BTM天线一般在第一车轴后某个位置
@ -29,20 +30,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,41 +38,78 @@ type BtmAntennaToBaliseInfo struct {
type BtmAntennaScanningBaliseInfo struct {
BaliseId string //应答器id
Time time.Time //应答器预计被BTM天线激活的时刻
active bool //true-激活过，即列车扫过
}
// BaliseDetector 车载BTM天线，应答器探测器
type BaliseDetector struct {
basbi *BtmAntennaScanningBaliseInfo
basbiLock sync.Mutex
eq [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头
eqLock sync.Mutex
last *BtmAntennaScanningBaliseInfo
}
func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
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)
curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)
if curExpect != nil {
dt := curExpect.Time.UnixMilli() - curTime.UnixMilli()
if dt <= 20 {
//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
t.addExpectedBalise(curExpect)
}
}
}
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
func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) {
if curExpect == nil {
return
}
return nil
//
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
}
}
//左移
for i := 1; i < len(t.eq); i++ {
t.eq[i-1] = t.eq[i]
}
//存入队尾
t.eq[len(t.eq)-1] = curExpect
}
func (t *BaliseDetector) updateBasbi(basbi *BtmAntennaScanningBaliseInfo) {
t.basbiLock.Lock()
defer t.basbiLock.Unlock()
func (t *BaliseDetector) DoScan() *BtmAntennaScanningBaliseInfo {
//
if basbi != nil {
*t.basbi = *basbi
} else {
t.basbi = nil
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) HasBaliseBeingScan() bool {
//
t.eqLock.Lock()
defer t.eqLock.Unlock()
//
for i := 0; i < len(t.eq); i++ {
if t.eq[i] != nil && !t.eq[i].active {
return true
}
}
return false
}
// 计算列车在当前运行状态下，预测到最近一个应答器的时刻
@ -225,6 +250,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 +269,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 +316,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,