diff --git a/third_party/can_btm/balise_detection.go b/third_party/can_btm/balise_detection.go new file mode 100644 index 0000000..2060f24 --- /dev/null +++ b/third_party/can_btm/balise_detection.go @@ -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 +}