339 lines
9.8 KiB
Go
339 lines
9.8 KiB
Go
package can_btm
|
||
|
||
import (
|
||
"fmt"
|
||
"joylink.club/bj-rtsts-server/config"
|
||
"joylink.club/bj-rtsts-server/third_party/message"
|
||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||
"joylink.club/ecs"
|
||
"joylink.club/rtsssimulation/entity"
|
||
"log/slog"
|
||
"sort"
|
||
"sync"
|
||
"time"
|
||
)
|
||
|
||
//使用：当有多个虚拟车时，同一时刻只有一个虚拟车与CANET关联
|
||
|
||
// BtmCanetManager BTM CANET 管理器
|
||
type BtmCanetManager interface {
|
||
EvnWorld() ecs.World
|
||
//GetBtmCanetConfig 获取CANET配置信息
|
||
GetBtmCanetConfig() config.BtmCanetConfig
|
||
}
|
||
|
||
// btm与canet(网口-CAN口转换器)
|
||
type btmCanetClient struct {
|
||
bcm BtmCanetManager
|
||
//udp server
|
||
udpServer udp.UdpServer
|
||
//udp client
|
||
udpClient udp.UdpClient
|
||
//udp 本地侦听端口
|
||
localUdpPort int
|
||
//udp 远程端口
|
||
remoteUdpPort int
|
||
//udp 远程ip
|
||
remoteIp string
|
||
//最近一次车载ATP系统查询帧序号
|
||
atpReqSn byte
|
||
//btm最近一次接收到atp查询请求的时间
|
||
//当BTM超过150ms即3个周期没有收到atq下发的查询帧，则清空报文缓冲区
|
||
atpReqTime *time.Time
|
||
//最近一次车载ATP系统查询帧CRC16校验结果,true-校验通过
|
||
atpReqCrc16Check bool
|
||
//btm系统时间,每次接收到ATP查询请求帧时同步一次时间
|
||
btmTime btmClock
|
||
//数据流水号
|
||
dsn byte
|
||
//重发的数据
|
||
resendData *resendData
|
||
//车载BTM天线，探测应答器
|
||
baliseDetector *BaliseDetector
|
||
}
|
||
type btmClock struct {
|
||
btmTk uint32 //与ATP系统同步的时间ms
|
||
sysTk time.Time //本地系统时间
|
||
}
|
||
|
||
// 获取以btmTk为基准的当前时间ms
|
||
func (c *btmClock) tkNow() uint32 {
|
||
return c.btmTk + uint32(time.Now().UnixMilli()-c.sysTk.UnixMilli())
|
||
}
|
||
|
||
type BtmCanetClient interface {
|
||
Start(bcm BtmCanetManager)
|
||
Stop()
|
||
//HandleTrainHeadPositionInfo 处理收到列车位置信息
|
||
HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo)
|
||
}
|
||
|
||
var (
|
||
btmClientLocker sync.Mutex
|
||
btmClient BtmCanetClient
|
||
)
|
||
|
||
func Default() BtmCanetClient {
|
||
btmClientLocker.Lock()
|
||
defer btmClientLocker.Unlock()
|
||
if btmClient == nil {
|
||
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()
|
||
//测试用
|
||
//cfg.Open = true
|
||
//cfg.RemoteUdpPort = 5555
|
||
//cfg.RemoteIp = "192.168.3.5"
|
||
//cfg.LocalUdpPort = 6666
|
||
//
|
||
if !cfg.Open {
|
||
return
|
||
}
|
||
s.localUdpPort = cfg.LocalUdpPort
|
||
s.remoteIp = cfg.RemoteIp
|
||
s.remoteUdpPort = cfg.RemoteUdpPort
|
||
//
|
||
s.udpServer = udp.NewServer(fmt.Sprintf(":%d", s.localUdpPort), s.handleCanetFrames)
|
||
s.udpServer.Listen()
|
||
//
|
||
s.udpClient = udp.NewClient(fmt.Sprintf("%s:%d", s.remoteIp, s.remoteUdpPort))
|
||
}
|
||
|
||
func (s *btmCanetClient) Stop() {
|
||
if s.udpServer != nil {
|
||
s.udpServer.Close()
|
||
s.udpServer = nil
|
||
}
|
||
if s.udpClient != nil {
|
||
s.udpClient.Close()
|
||
s.udpClient = nil
|
||
}
|
||
}
|
||
func (s *btmCanetClient) handleCanetFrames(cfs []byte) {
|
||
defer func() {
|
||
if e := recover(); e != nil {
|
||
slog.Debug(fmt.Sprintf("handleCanetFrames异常[%s]", e))
|
||
}
|
||
}()
|
||
//一个cannet 帧 13字节
|
||
if len(cfs) > 0 && len(cfs)%13 == 0 {
|
||
cfSum := len(cfs) / 13
|
||
dms := make([]*message.CanetFrame, 0, 16) //13个应答器报文数据帧+TimeA帧+TimeB帧+结束帧
|
||
for cfi := 0; cfi < cfSum; cfi++ {
|
||
cfStart := cfi * 13
|
||
cf := message.NewCanetFrame(cfs[cfStart : cfStart+13])
|
||
//
|
||
switch cf.CanFrameType() {
|
||
case message.CfReq:
|
||
s.dealWithAptReq(cf)
|
||
case message.CfStatusRsp:
|
||
s.dealWithBtmStatusRsp(cf)
|
||
case message.CfTimeSync:
|
||
s.dealWithBtmTimeSyncRsp(cf)
|
||
case message.CfMsg:
|
||
fallthrough
|
||
case message.CfMsgTimeA:
|
||
fallthrough
|
||
case message.CfMsgTimeB:
|
||
fallthrough
|
||
case message.CfMsgEnd:
|
||
dms = append(dms, cf)
|
||
default:
|
||
slog.Warn("CanetFrame帧没有具体对应的应用帧", "CannetFrame", cf.String())
|
||
} //switch
|
||
} //for
|
||
//将数据包按ID3即0x80+offset由小到大排序
|
||
sort.SliceStable(dms, func(i, j int) bool {
|
||
return dms[i].CanId.ID3 < dms[j].CanId.ID3
|
||
})
|
||
//有数据帧，但是不足16帧
|
||
if len(dms) > 0 {
|
||
if len(dms) != 16 {
|
||
slog.Warn("接收到数据帧，但数据帧数量不足16帧")
|
||
} else {
|
||
s.dealWithBtmDataFrames(dms)
|
||
}
|
||
}
|
||
|
||
} else {
|
||
slog.Warn("从cannet接收数据，未满足条件‘len(cfs) > 0 && len(cfs)%13 == 0‘", "len(cfs)", len(cfs))
|
||
}
|
||
}
|
||
|
||
// 处理接收的ATP查询请求帧
|
||
func (s *btmCanetClient) dealWithAptReq(f *message.CanetFrame) {
|
||
now := time.Now()
|
||
atpReq := &message.AtpRequestFrame{}
|
||
if !atpReq.Decode(f) {
|
||
slog.Warn("CanetFrame解码成AtpRequestFrame失败", "CanetFrame", f.String())
|
||
return
|
||
}
|
||
//处理查询请求
|
||
//slog.Debug(fmt.Sprintf("处理查询请求:%s", atpReq.String()))
|
||
//
|
||
s.btmTime.btmTk = atpReq.Time
|
||
s.btmTime.sysTk = time.Now()
|
||
s.atpReqSn = atpReq.FId.ID4
|
||
s.atpReqCrc16Check = atpReq.Crc16CheckOk
|
||
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) rspResendToAtp() {
|
||
//重发上一报文处理
|
||
if s.resendData != nil && s.resendData.canResend() {
|
||
s.resendData.countAdd1()
|
||
s.sendCanetFrame(s.resendData.data)
|
||
return
|
||
}
|
||
}
|
||
|
||
// BTM发送响应给ATP
|
||
// 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
|
||
// 当未收到应答器报文时响应：时间同步帧、状态应答帧
|
||
func (s *btmCanetClient) rspToAtp(sb *BtmAntennaScanningBaliseInfo) {
|
||
//BTM状态
|
||
statusF := message.NewBtmStatusRspFrame(s.atpReqSn)
|
||
btmStatus := s.createTrainBtmStatus()
|
||
|
||
statusF.AntennaFault = btmStatus.AntennaFault
|
||
statusF.BaliseCounter = byte(btmStatus.BaliseCounter)
|
||
statusF.MessageCounter = byte(btmStatus.MessageCounter)
|
||
statusF.PowerAmplifierOn = btmStatus.PowerAmplifierOn
|
||
statusF.TkTimeA = s.btmTime.tkNow()
|
||
statusF.PowerAmplifierFailure = btmStatus.PowerAmplifierFault
|
||
statusF.DetailedCode = 0
|
||
if btmStatus.AboveBalise {
|
||
statusF.DetailedCode = 0x07
|
||
}
|
||
statusF.AtpReqCrcCheckWrong = !s.atpReqCrc16Check
|
||
statusF.Dsn = s.dsn
|
||
s.dsnAdd1()
|
||
//
|
||
//true-收到应答器报文
|
||
isRcvTelegram := sb != nil && len(sb.telegram) > 0
|
||
if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
|
||
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
|
||
timeSyncF.T3 = s.btmTime.tkNow()
|
||
s.sendCanetFrame(timeSyncF.Encode().Encode())
|
||
//
|
||
s.resendData = newResendData(statusDataCf)
|
||
s.sendCanetFrame(statusDataCf)
|
||
} else {
|
||
slog.Warn("BtmCanetClient应答帧、数据帧编码失败")
|
||
}
|
||
} else { //当未收到应答器报文时响应：时间同步帧、状态应答帧
|
||
timeSyncF := message.NewBtmTimeSyncCheckFrame(s.atpReqSn)
|
||
timeSyncF.T2 = s.btmTime.btmTk
|
||
timeSyncF.T3 = s.btmTime.tkNow()
|
||
s.sendCanetFrame(timeSyncF.Encode().Encode())
|
||
//
|
||
statusCf := statusF.Encode().Encode()
|
||
s.sendCanetFrame(statusCf)
|
||
}
|
||
}
|
||
|
||
// 发送Canet帧，一帧13字节
|
||
func (s *btmCanetClient) sendCanetFrame(cf []byte) {
|
||
s.udpClient.Send(cf)
|
||
}
|
||
func (s *btmCanetClient) dsnAdd1() {
|
||
if s.dsn >= 255 {
|
||
s.dsn = 0
|
||
} else {
|
||
s.dsn++
|
||
}
|
||
}
|
||
|
||
// 准备重发的状态应答帧和报文数据帧
|
||
type resendData struct {
|
||
data []byte //重发的数据
|
||
count int //重发次数
|
||
}
|
||
|
||
func newResendData(data []byte) *resendData {
|
||
return &resendData{
|
||
data: data,
|
||
count: 0,
|
||
}
|
||
}
|
||
func (r *resendData) canResend() bool {
|
||
return r.count <= 3
|
||
}
|
||
func (r *resendData) countAdd1() {
|
||
r.count++
|
||
}
|
||
|
||
//////////////////////////////////////////////////////////////////////////////////////////
|
||
|
||
// 处理接收的状态应答帧
|
||
func (s *btmCanetClient) dealWithBtmStatusRsp(f *message.CanetFrame) {
|
||
slog.Debug(fmt.Sprintf("接收到状态应答帧%s", f.String()))
|
||
}
|
||
|
||
// 处理接收的时间同步校验应答帧
|
||
func (s *btmCanetClient) dealWithBtmTimeSyncRsp(f *message.CanetFrame) {
|
||
slog.Debug(fmt.Sprintf("接收到时间同步校验应答帧%s", f.String()))
|
||
}
|
||
|
||
// 处理接收到的16帧数据帧
|
||
func (s *btmCanetClient) dealWithBtmDataFrames(dms []*message.CanetFrame) {
|
||
for _, dm := range dms {
|
||
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 //报文计数器（每解出一个应答器报文加一）
|
||
}
|