Merge branch 'master' of https://git.code.tencent.com/beijing-rtss-test/bj-rtsts-server-go
This commit is contained in:
weizhihong
commit
745fb55110
91
third_party/can_btm/balise_btm.go
vendored
91
third_party/can_btm/balise_btm.go
vendored
@ -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 //报文计数器（每解出一个应答器报文加一）
|
||||
}
|
||||
|
||||
176
third_party/can_btm/balise_detection.go
vendored
176
third_party/can_btm/balise_detection.go
vendored
@ -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)
|
||||
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() //报文计数器
|
||||
}
|
||||
}
|
||||
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
|
||||
//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
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user