2023-12-01 15:38:26 +08:00
|
|
|
|
package can_btm
|
|
|
|
|
|
|
|
|
|
import (
|
|
|
|
|
"fmt"
|
2024-06-06 17:57:30 +08:00
|
|
|
|
"joylink.club/bj-rtsts-server/dto/state_proto"
|
|
|
|
|
"joylink.club/bj-rtsts-server/third_party/btm_vobc"
|
2023-12-01 15:38:26 +08:00
|
|
|
|
"joylink.club/rtsssimulation/component"
|
2023-12-05 09:37:34 +08:00
|
|
|
|
"joylink.club/rtsssimulation/fi"
|
2023-12-01 15:38:26 +08:00
|
|
|
|
"joylink.club/rtsssimulation/repository"
|
|
|
|
|
"joylink.club/rtsssimulation/repository/model/proto"
|
2023-12-05 13:49:22 +08:00
|
|
|
|
"log/slog"
|
2023-12-01 15:38:26 +08:00
|
|
|
|
"math"
|
|
|
|
|
"sort"
|
|
|
|
|
"sync"
|
|
|
|
|
"time"
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
//通过提前预测来实现BTM天线扫描应答器的实时性
|
2023-12-05 09:37:34 +08:00
|
|
|
|
//根据当前列车运行信息预测出列车前方应答器被扫描到的时刻
|
2023-12-01 15:38:26 +08:00
|
|
|
|
|
|
|
|
|
// BtmAntennaRunningInfo 车载BTM天线中心点位置运行信息
|
|
|
|
|
// BTM天线一般在第一车轴后某个位置
|
|
|
|
|
type BtmAntennaRunningInfo struct {
|
|
|
|
|
Up bool //车载BTM天线中心点运行方向
|
|
|
|
|
LinkId string //车载BTM天线中心点所在轨道的id
|
|
|
|
|
LinkOffset int64 //车载BTM天线中心点所在轨道上的偏移,mm
|
|
|
|
|
Speed float32 //列车运行速度(m/s)
|
|
|
|
|
Acceleration float32 //加速度(m/s^2)
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const (
|
|
|
|
|
BtmAntennaOffsetHead = int64(1000) //车载BTM天线距车头端点的距离,mm
|
|
|
|
|
)
|
|
|
|
|
|
|
|
|
|
// TrainHeadPositionInfo 列车车头运行位置信息
|
2023-12-05 09:37:34 +08:00
|
|
|
|
type TrainHeadPositionInfo = fi.TrainHeadPositionInfo
|
2023-12-01 15:38:26 +08:00
|
|
|
|
type BtmAntennaToBaliseInfo struct {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
Distance int64 //BTM天线中心到应答器的距离,mm
|
|
|
|
|
BaliseId string //应答器id
|
|
|
|
|
BaliseType proto.Transponder_Type //应答器类型
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
type BtmAntennaScanningBaliseInfo struct {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
BaliseId string //应答器id
|
|
|
|
|
Time time.Time //应答器预计被BTM天线激活的时刻
|
|
|
|
|
active bool //true-激活过,即列车扫过
|
|
|
|
|
telegram []byte //应答器报文
|
|
|
|
|
Distance int64 //BTM天线中心到应答器的距离,mm
|
|
|
|
|
BaliseType proto.Transponder_Type //应答器类型
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
2023-12-05 09:37:34 +08:00
|
|
|
|
|
|
|
|
|
// BaliseDetector 车载BTM天线,应答器探测器
|
2023-12-01 15:38:26 +08:00
|
|
|
|
type BaliseDetector struct {
|
2023-12-05 09:37:34 +08:00
|
|
|
|
eq [3]*BtmAntennaScanningBaliseInfo //预测将被BTM天线扫描的应答器队列,左边为头
|
|
|
|
|
eqLock sync.Mutex
|
2023-12-05 10:57:31 +08:00
|
|
|
|
//车载应答器天线功率放大器开关,true-开,false-关
|
|
|
|
|
powerAmplifierSwitch bool
|
|
|
|
|
//天线此时是否在应答器上方
|
|
|
|
|
aboveBalise bool
|
|
|
|
|
//应答器计数(每过一个应答器加一,在同一个应答器内不变)
|
|
|
|
|
baliseCounter int
|
|
|
|
|
//报文计数器(每解出一个应答器报文加一,应答器报文长度830bits)
|
|
|
|
|
messageCounter int
|
2023-12-05 13:49:22 +08:00
|
|
|
|
//BTM所在列车id
|
|
|
|
|
trianId string
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
|
2023-12-05 13:49:22 +08:00
|
|
|
|
// 由于同一时间只能有一辆列车与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))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
2024-06-06 17:57:30 +08:00
|
|
|
|
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
|
2023-12-05 13:49:22 +08:00
|
|
|
|
t.tryRebind(th)
|
2024-02-21 14:54:17 +08:00
|
|
|
|
//if !t.powerAmplifierSwitch { //天线功率放大器未开启,不进行探测
|
|
|
|
|
// return
|
|
|
|
|
//}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
curTime := time.Now()
|
2023-12-05 09:37:34 +08:00
|
|
|
|
//BTM天线中心点运行信息
|
|
|
|
|
curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)
|
2023-12-01 15:38:26 +08:00
|
|
|
|
//预测BTM天线到最近一个应答器的时刻
|
2023-12-05 09:37:34 +08:00
|
|
|
|
curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)
|
2024-06-06 17:57:30 +08:00
|
|
|
|
|
2023-12-05 10:57:31 +08:00
|
|
|
|
if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms
|
|
|
|
|
//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
|
2024-06-06 17:57:30 +08:00
|
|
|
|
telegram := t.rcvTelegram(wd, curExpect.BaliseId)
|
|
|
|
|
|
|
|
|
|
if curExpect.Distance <= 50 {
|
|
|
|
|
btm_vobc.Default().AppendBaliseMsgForTrain(vobcBtm, curExpect.BaliseId, telegram, curTime.UnixMilli())
|
|
|
|
|
}
|
2023-12-05 10:57:31 +08:00
|
|
|
|
//记录即将经过的应答器
|
|
|
|
|
if t.addExpectedBalise(curExpect) {
|
|
|
|
|
t.baliseCounterAdd1() //应答器计数器
|
|
|
|
|
if len(telegram) > 0 {
|
|
|
|
|
curExpect.telegram = telegram
|
|
|
|
|
t.baliseMessageCounterAdd1() //报文计数器
|
2024-06-06 17:57:30 +08:00
|
|
|
|
|
2023-12-05 10:57:31 +08:00
|
|
|
|
}
|
2023-12-05 09:37:34 +08:00
|
|
|
|
}
|
2023-12-05 10:57:31 +08:00
|
|
|
|
//BTM天线即将经过应答器
|
|
|
|
|
t.aboveBalise = true
|
|
|
|
|
} else {
|
|
|
|
|
t.aboveBalise = false
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
2024-06-06 17:57:30 +08:00
|
|
|
|
|
|
|
|
|
curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, &TrainHeadPositionInfo{TrainId: th.TrainId,
|
|
|
|
|
Up: !th.Up,
|
|
|
|
|
Link: th.Link,
|
|
|
|
|
LinkOffset: th.LinkOffset,
|
|
|
|
|
Speed: th.Speed,
|
|
|
|
|
Acceleration: th.Acceleration})
|
|
|
|
|
curExpect2 := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi2)
|
|
|
|
|
if curExpect2 != nil && curExpect2.Distance > 20 {
|
|
|
|
|
btm_vobc.Default().UpdateTrainLeave(vobcBtm, curExpect2.BaliseId, curTime.UnixMilli())
|
|
|
|
|
}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
2023-12-05 10:57:31 +08:00
|
|
|
|
|
|
|
|
|
// 应答器计数器加1,[0,255]
|
|
|
|
|
func (t *BaliseDetector) baliseCounterAdd1() {
|
|
|
|
|
t.baliseCounter++
|
|
|
|
|
if t.baliseCounter > 255 {
|
|
|
|
|
t.baliseCounter = 0
|
2023-12-05 09:37:34 +08:00
|
|
|
|
}
|
2023-12-05 10:57:31 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 报文计数器加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]
|
2024-02-06 09:30:58 +08:00
|
|
|
|
if ok {
|
|
|
|
|
if component.BaliseWorkStateType.Get(entry).Work {
|
|
|
|
|
return component.BaliseFixedTelegramType.Get(entry).Telegram
|
|
|
|
|
}
|
2023-12-05 10:57:31 +08:00
|
|
|
|
}
|
2024-02-06 09:30:58 +08:00
|
|
|
|
return nil
|
2023-12-05 10:57:31 +08:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// true-新增;false-更新
|
|
|
|
|
func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) bool {
|
2023-12-01 15:38:26 +08:00
|
|
|
|
//
|
2023-12-05 09:37:34 +08:00
|
|
|
|
t.eqLock.Lock()
|
|
|
|
|
defer t.eqLock.Unlock()
|
2024-02-06 16:39:56 +08:00
|
|
|
|
////更新,去重
|
|
|
|
|
//for i, e := range t.eq {
|
|
|
|
|
// if e != nil && e.BaliseId == curExpect.BaliseId {
|
|
|
|
|
// t.eq[i].Time = curExpect.Time
|
|
|
|
|
// return false
|
|
|
|
|
// }
|
|
|
|
|
//}
|
|
|
|
|
//检查是否已经记录过
|
2024-02-21 14:54:17 +08:00
|
|
|
|
eq := t.eq[len(t.eq)-1]
|
|
|
|
|
if eq != nil && eq.BaliseId == curExpect.BaliseId {
|
2024-02-06 16:39:56 +08:00
|
|
|
|
return false
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
2023-12-05 09:37:34 +08:00
|
|
|
|
//左移
|
|
|
|
|
for i := 1; i < len(t.eq); i++ {
|
|
|
|
|
t.eq[i-1] = t.eq[i]
|
|
|
|
|
}
|
|
|
|
|
//存入队尾
|
|
|
|
|
t.eq[len(t.eq)-1] = curExpect
|
2023-12-05 10:57:31 +08:00
|
|
|
|
return true
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
2023-12-05 13:49:22 +08:00
|
|
|
|
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 {
|
2023-12-01 15:38:26 +08:00
|
|
|
|
//
|
2023-12-05 09:37:34 +08:00
|
|
|
|
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
|
|
|
|
|
}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
|
|
|
|
|
// 计算列车在当前运行状态下,预测到最近一个应答器的时刻
|
|
|
|
|
func (t *BaliseDetector) timeScanNearestBalise(curTime time.Time, wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaScanningBaliseInfo {
|
|
|
|
|
expectedBalise := t.findBaliseWillScanByBtmAntenna(wd, repo, ba)
|
|
|
|
|
if expectedBalise != nil {
|
|
|
|
|
curV := float64(ba.Speed)
|
|
|
|
|
curAc := float64(ba.Acceleration)
|
|
|
|
|
s := float64(expectedBalise.Distance) / 1000
|
|
|
|
|
st, ok := t.calculateBtmAntennaScanNextBaliseTime(curTime, curV, curAc, s)
|
|
|
|
|
if ok {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
return &BtmAntennaScanningBaliseInfo{BaliseId: expectedBalise.BaliseId, Time: st, Distance: expectedBalise.Distance, BaliseType: expectedBalise.BaliseType}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return nil
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 预测BTM天线到运行方向的最近应答器的时刻
|
|
|
|
|
// curV-当前时刻BTM天线速度,m/s
|
|
|
|
|
// curAc-当前时刻BTM天线加速度,m/s^2
|
|
|
|
|
// s-BTM天线从当前时刻所处位置到运行方向最近一个应答器的位移,m
|
|
|
|
|
func (t *BaliseDetector) calculateBtmAntennaScanNextBaliseTime(curTime time.Time, curV float64, curAc float64, s float64) (time.Time, bool) {
|
|
|
|
|
a := 0.5 * curAc
|
|
|
|
|
b := curV
|
|
|
|
|
c := -s
|
|
|
|
|
//nt 单位秒
|
|
|
|
|
nt, ok := t.calculateQuadratic(a, b, c)
|
|
|
|
|
if ok {
|
|
|
|
|
return curTime.Add(time.Millisecond * time.Duration(nt*1000)), true
|
|
|
|
|
}
|
|
|
|
|
return curTime, false
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 获取车载BTM天线中心点运行方向最近的1个应答器
|
|
|
|
|
func (t *BaliseDetector) findBaliseWillScanByBtmAntenna(wd *component.WorldData, repo *repository.Repository, ba *BtmAntennaRunningInfo) *BtmAntennaToBaliseInfo {
|
|
|
|
|
//BTM天线中心点所在轨道
|
|
|
|
|
baLink := repo.FindLink(ba.LinkId)
|
|
|
|
|
rs1 := t.searchBalisesFromLinkPosition(repo, ba.LinkId, ba.Up, ba.LinkOffset)
|
|
|
|
|
if ba.Up {
|
|
|
|
|
if len(rs1) > 0 {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
rs := rs1[0]
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: rs.LinkPosition().Offset() - ba.LinkOffset, BaliseType: rs.BaliseType()}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
} else {
|
|
|
|
|
nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
|
|
|
|
|
if nextLinkPort != nil {
|
|
|
|
|
if nextLinkPort.IsPortA() {
|
|
|
|
|
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
|
|
|
|
|
if len(rs2) > 0 {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
rs := rs2[0]
|
|
|
|
|
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
|
|
|
|
|
if len(rs2) > 0 {
|
2024-06-06 17:57:30 +08:00
|
|
|
|
rs := rs2[0]
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
if len(rs1) > 0 {
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs1[0].Id(), Distance: ba.LinkOffset - rs1[0].LinkPosition().Offset()}
|
|
|
|
|
} else {
|
|
|
|
|
nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
|
|
|
|
|
if nextLinkPort != nil {
|
|
|
|
|
if nextLinkPort.IsPortA() {
|
|
|
|
|
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
|
|
|
|
|
if len(rs2) > 0 {
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + rs2[0].LinkPosition().Offset()}
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
|
|
|
|
|
if len(rs2) > 0 {
|
|
|
|
|
return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: ba.LinkOffset + nextLinkPort.Link().Length() - rs2[0].LinkPosition().Offset()}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return nil
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// up-在轨道上的搜索方向
|
|
|
|
|
func (t *BaliseDetector) searchBalisesFromLinkPosition(repo *repository.Repository, linkId string, up bool, fromOffset int64) []*repository.Transponder {
|
|
|
|
|
rs := repo.ResponderListByLink(linkId)
|
|
|
|
|
if up {
|
|
|
|
|
sort.SliceStable(rs, func(i, j int) bool {
|
|
|
|
|
return rs[i].LinkPosition().Offset() < rs[j].LinkPosition().Offset()
|
|
|
|
|
})
|
|
|
|
|
//
|
|
|
|
|
for i, r := range rs {
|
|
|
|
|
if r.LinkPosition().Offset() >= fromOffset {
|
|
|
|
|
return rs[i:]
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
sort.SliceStable(rs, func(i, j int) bool {
|
|
|
|
|
return rs[j].LinkPosition().Offset() < rs[i].LinkPosition().Offset()
|
|
|
|
|
})
|
|
|
|
|
for i, r := range rs {
|
|
|
|
|
if r.LinkPosition().Offset() <= fromOffset {
|
|
|
|
|
return rs[i:]
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return nil
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 列车车头端点运行信息转换为车载BTM天线中心点运行信息
|
|
|
|
|
func (t *BaliseDetector) createBtmAntennaRunningInfo(wd *component.WorldData, repo *repository.Repository, head *TrainHeadPositionInfo) *BtmAntennaRunningInfo {
|
|
|
|
|
headLink := repo.FindLink(head.Link)
|
|
|
|
|
if head.Up {
|
|
|
|
|
if head.LinkOffset >= BtmAntennaOffsetHead { //车头与BTM天线在同一个轨道上
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: head.Up, LinkId: head.Link, LinkOffset: head.LinkOffset - BtmAntennaOffsetHead, Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
} else { //车头与BTM天线在同一个轨道上
|
|
|
|
|
nextLinkPort := t.getNextLink(wd, repo, head.Link, !head.Up)
|
|
|
|
|
nextLink := nextLinkPort.Link()
|
|
|
|
|
if nextLinkPort.IsPortA() {
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: false, LinkId: nextLink.Id(), LinkOffset: BtmAntennaOffsetHead - head.LinkOffset, Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
} else {
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: true, LinkId: nextLink.Id(), LinkOffset: nextLink.Length() - (BtmAntennaOffsetHead - head.LinkOffset), Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
if headLink.Length()-head.LinkOffset >= BtmAntennaOffsetHead { //车头与BTM天线在同一个轨道上
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: head.Up, LinkId: head.Link, LinkOffset: head.LinkOffset + BtmAntennaOffsetHead, Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
} else {
|
|
|
|
|
nextLinkPort := t.getNextLink(wd, repo, head.Link, !head.Up)
|
|
|
|
|
nextLink := nextLinkPort.Link()
|
|
|
|
|
if nextLinkPort.IsPortA() {
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: false, LinkId: nextLink.Id(), LinkOffset: BtmAntennaOffsetHead - headLink.Length() + head.LinkOffset, Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
} else {
|
|
|
|
|
return &BtmAntennaRunningInfo{Up: true, LinkId: nextLink.Id(), LinkOffset: nextLink.Length() - (BtmAntennaOffsetHead - headLink.Length() + head.LinkOffset), Speed: head.Speed, Acceleration: head.Acceleration}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
func (t *BaliseDetector) getNextLink(wd *component.WorldData, repo *repository.Repository, curLinkId string, searchDirection bool) *repository.LinkPort {
|
|
|
|
|
var nextLinkPort *repository.LinkPort
|
|
|
|
|
curLink := repo.FindLink(curLinkId)
|
|
|
|
|
if searchDirection {
|
|
|
|
|
bDc := curLink.BRelation()
|
2023-12-05 09:37:34 +08:00
|
|
|
|
if bDc == nil || bDc.Turnout() == nil {
|
|
|
|
|
return nil
|
|
|
|
|
}
|
|
|
|
|
//
|
2023-12-01 15:38:26 +08:00
|
|
|
|
bDcDw := t.turnoutPosition(wd, bDc.Turnout().Id())
|
|
|
|
|
switch bDc.Port() {
|
|
|
|
|
case proto.Port_A: //link-b连接turnout-A
|
|
|
|
|
if bDcDw {
|
|
|
|
|
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_B)
|
|
|
|
|
} else {
|
|
|
|
|
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_C)
|
|
|
|
|
}
|
|
|
|
|
case proto.Port_B: //link-b连接turnout-B
|
|
|
|
|
fallthrough
|
|
|
|
|
case proto.Port_C: //link-b连接turnout-C
|
|
|
|
|
nextLinkPort = bDc.Turnout().FindLinkByPort(proto.Port_A)
|
|
|
|
|
}
|
|
|
|
|
} else {
|
|
|
|
|
aDc := curLink.ARelation()
|
2023-12-05 09:37:34 +08:00
|
|
|
|
if aDc == nil || aDc.Turnout() == nil {
|
|
|
|
|
return nil
|
|
|
|
|
}
|
|
|
|
|
//
|
2023-12-01 15:38:26 +08:00
|
|
|
|
aDcDw := t.turnoutPosition(wd, aDc.Turnout().Id())
|
|
|
|
|
switch aDc.Port() {
|
|
|
|
|
case proto.Port_A: //link-a连接turnout-A
|
|
|
|
|
if aDcDw {
|
|
|
|
|
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_B)
|
|
|
|
|
} else {
|
|
|
|
|
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_C)
|
|
|
|
|
}
|
|
|
|
|
case proto.Port_B: //link-a连接turnout-B
|
|
|
|
|
fallthrough
|
|
|
|
|
case proto.Port_C: //link-a连接turnout-C
|
|
|
|
|
nextLinkPort = aDc.Turnout().FindLinkByPort(proto.Port_A)
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
//
|
|
|
|
|
return nextLinkPort
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 获取道岔实际位置(线程不安全)
|
|
|
|
|
func (t *BaliseDetector) turnoutPosition(wd *component.WorldData, id string) bool {
|
|
|
|
|
entry, ok := wd.EntityMap[id]
|
|
|
|
|
if ok {
|
|
|
|
|
return component.TurnoutPositionType.Get(entry).Dw
|
|
|
|
|
} else {
|
|
|
|
|
panic(fmt.Sprintf("道岔[%s]的实体不存在", id))
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// 解 ax²+bx+c=0
|
|
|
|
|
func (t *BaliseDetector) calculateQuadratic(a, b, c float64) (float64, bool) {
|
|
|
|
|
//D=b²-4ac
|
|
|
|
|
d := b*b - 4*a*c
|
|
|
|
|
if d < 0 {
|
|
|
|
|
return 0, false
|
|
|
|
|
} else if d == 0 { //若D=0,则计算它的重根:x=-b/2a
|
|
|
|
|
return -(b / (2 * a)), true
|
|
|
|
|
} else { //若D>0,则计算它的两个根:x1=(-b+√D)/2a,x2=(-b-√D)/2a
|
|
|
|
|
sqrtD := math.Sqrt(d)
|
|
|
|
|
ax2 := 2 * a
|
|
|
|
|
//选择正直输出
|
|
|
|
|
x1 := (-b + sqrtD) / ax2
|
|
|
|
|
x2 := (-b - sqrtD) / ax2
|
|
|
|
|
if x1 >= 0 && x2 >= 0 {
|
2023-12-05 09:37:34 +08:00
|
|
|
|
//panic("x1 >= 0 && x2 >= 0")
|
|
|
|
|
return 0, false
|
2023-12-01 15:38:26 +08:00
|
|
|
|
}
|
|
|
|
|
if x1 >= 0 {
|
|
|
|
|
return x1, true
|
|
|
|
|
}
|
|
|
|
|
if x2 >= 0 {
|
|
|
|
|
return x2, true
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return 0, false
|
|
|
|
|
}
|