balise detect 预测实现
This commit is contained in:
xzb
parent
163c0f53c2
commit
83a5ee52e8
54
third_party/can_btm/balise_btm.go
vendored
54
third_party/can_btm/balise_btm.go
vendored
@ -6,7 +6,6 @@ import (
|
||||
"joylink.club/bj-rtsts-server/third_party/message"
|
||||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||||
"joylink.club/ecs"
|
||||
"joylink.club/rtsssimulation/component"
|
||||
"joylink.club/rtsssimulation/entity"
|
||||
"log/slog"
|
||||
"sort"
|
||||
@ -51,14 +50,6 @@ type btmCanetClient struct {
|
||||
resendData *resendData
|
||||
//车载BTM天线，探测应答器
|
||||
baliseDetector *BaliseDetector
|
||||
//车载应答器天线功率放大器开关，true-开，false-关
|
||||
powerAmplifierSwitch bool
|
||||
//天线此时是否在应答器上方
|
||||
aboveBalise bool
|
||||
//应答器计数（每过一个应答器加一，在同一个应答器内不变）
|
||||
baliseCounter int
|
||||
//报文计数器（每解出一个应答器报文加一）
|
||||
messageCounter int
|
||||
}
|
||||
type btmClock struct {
|
||||
btmTk uint32 //与ATP系统同步的时间ms
|
||||
@ -90,22 +81,6 @@ func Default() BtmCanetClient {
|
||||
}
|
||||
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)
|
||||
@ -210,12 +185,11 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) {
|
||||
s.btmTime.sysTk = time.Now()
|
||||
s.atpReqSn = atpReq.FId.ID4
|
||||
s.atpReqCrc16Check = atpReq.Crc16CheckOk
|
||||
s.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn
|
||||
s.baliseDetector.powerAmplifierSwitch = atpReq.PowerAmplifierTurnOn
|
||||
//记录atp查询时间
|
||||
s.atpReqTime = &now
|
||||
//ATP 是否要求BTM 重发上一应答器报文
|
||||
isResendRequest := atpReq.ResendRequest == 2 //0b10
|
||||
s.aboveBalise = s.baliseDetector.HasBaliseBeingScan()
|
||||
if isResendRequest {
|
||||
s.rspResendToAtp()
|
||||
} else {
|
||||
@ -223,29 +197,20 @@ func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) {
|
||||
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.powerAmplifierSwitch,
|
||||
PowerAmplifierOn: s.baliseDetector.powerAmplifierSwitch,
|
||||
PowerAmplifierFault: false,
|
||||
AboveBalise: s.aboveBalise,
|
||||
AboveBalise: s.baliseDetector.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
|
||||
BaliseCounter: s.baliseDetector.baliseCounter,
|
||||
MessageCounter: s.baliseDetector.messageCounter,
|
||||
}
|
||||
}
|
||||
|
||||
@ -283,11 +248,10 @@ func (s *btmCanetClient) rspToAtp(sb *BtmAntennaScanningBaliseInfo) {
|
||||
statusF.Dsn = s.dsn
|
||||
s.dsnAdd1()
|
||||
//
|
||||
baliseTelegram := s.findBaliseTelegram(sb.BaliseId)
|
||||
//true-收到应答器报文
|
||||
isRcvTelegram := baliseTelegram != nil
|
||||
isRcvTelegram := sb != nil && len(sb.telegram) > 0
|
||||
if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
|
||||
statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram, 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
|
||||
|
||||
69
third_party/can_btm/balise_detection.go
vendored
69
third_party/can_btm/balise_detection.go
vendored
@ -39,42 +39,86 @@ type BtmAntennaScanningBaliseInfo struct {
|
||||
BaliseId string //应答器id
|
||||
Time time.Time //应答器预计被BTM天线激活的时刻
|
||||
active bool //true-激活过，即列车扫过
|
||||
telegram []byte //应答器报文
|
||||
}
|
||||
|
||||
// BaliseDetector 车载BTM天线，应答器探测器
|
||||
type BaliseDetector struct {
|
||||
eq [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列，左边为头
|
||||
eqLock sync.Mutex
|
||||
|
||||
last *BtmAntennaScanningBaliseInfo
|
||||
//车载应答器天线功率放大器开关，true-开，false-关
|
||||
powerAmplifierSwitch bool
|
||||
//天线此时是否在应答器上方
|
||||
aboveBalise bool
|
||||
//应答器计数（每过一个应答器加一，在同一个应答器内不变）
|
||||
baliseCounter int
|
||||
//报文计数器（每解出一个应答器报文加一,应答器报文长度830bits）
|
||||
messageCounter int
|
||||
}
|
||||
|
||||
func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
|
||||
if !t.powerAmplifierSwitch { //天线功率放大器未不进行探测
|
||||
return
|
||||
}
|
||||
curTime := time.Now()
|
||||
//BTM天线中心点运行信息
|
||||
curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)
|
||||
//预测BTM天线到最近一个应答器的时刻
|
||||
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)
|
||||
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.aboveBalise = false
|
||||
}
|
||||
}
|
||||
func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) {
|
||||
if curExpect == nil {
|
||||
return
|
||||
|
||||
// 应答器计数器加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
|
||||
return false
|
||||
}
|
||||
}
|
||||
//左移
|
||||
@ -83,6 +127,7 @@ func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseIn
|
||||
}
|
||||
//存入队尾
|
||||
t.eq[len(t.eq)-1] = curExpect
|
||||
return true
|
||||
}
|
||||
func (t *BaliseDetector) DoScan() *BtmAntennaScanningBaliseInfo {
|
||||
//
|
||||
|
||||
Loading…
Reference in New Issue
Block a user