balise detect 预测实现
This commit is contained in:
xzb
parent
7c687972b4
commit
14af6608ea
296
third_party/can_btm/balise_detection.go
vendored
Normal file
296
third_party/can_btm/balise_detection.go
vendored
Normal file
@ -0,0 +1,296 @@
|
||||
package can_btm
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"joylink.club/rtsssimulation/component"
|
||||
"joylink.club/rtsssimulation/repository"
|
||||
"joylink.club/rtsssimulation/repository/model/proto"
|
||||
"log/slog"
|
||||
"math"
|
||||
"sort"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
|
||||
//通过提前预测来实现BTM天线扫描应答器的实时性
|
||||
|
||||
// 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 列车车头运行位置信息
|
||||
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 BtmAntennaToBaliseInfo struct {
|
||||
Distance int64 //BTM天线中心到应答器的距离，mm
|
||||
BaliseId string //应答器id
|
||||
}
|
||||
type BtmAntennaScanningBaliseInfo struct {
|
||||
BaliseId string //应答器id
|
||||
Time time.Time //应答器预计被BTM天线激活的时刻
|
||||
}
|
||||
type BaliseDetector struct {
|
||||
basbi *BtmAntennaScanningBaliseInfo
|
||||
basbiLock sync.Mutex
|
||||
}
|
||||
|
||||
func (t *BaliseDetector) Detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
|
||||
curTime := time.Now()
|
||||
bari := 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
|
||||
} else {
|
||||
t.basbi = nil
|
||||
}
|
||||
}
|
||||
|
||||
// 计算列车在当前运行状态下，预测到最近一个应答器的时刻
|
||||
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 {
|
||||
return &BtmAntennaScanningBaliseInfo{BaliseId: expectedBalise.BaliseId, Time: st}
|
||||
}
|
||||
}
|
||||
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 {
|
||||
return &BtmAntennaToBaliseInfo{BaliseId: rs1[0].Id(), Distance: rs1[0].LinkPosition().Offset() - ba.LinkOffset}
|
||||
} 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: baLink.Length() - 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: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs2[0].LinkPosition().Offset()}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
} 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()
|
||||
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()
|
||||
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 {
|
||||
panic("x1 >= 0 && x2 >= 0")
|
||||
}
|
||||
if x1 >= 0 {
|
||||
return x1, true
|
||||
}
|
||||
if x2 >= 0 {
|
||||
return x2, true
|
||||
}
|
||||
}
|
||||
return 0, false
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user