rts-sim-testing-service/third_party/can_btm/balise_btm.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/bj-rtsts-server/ts/simulation/wayside/memory"
	"joylink.club/rtsssimulation/fi"
	"log/slog"
	"sort"
	"sync"
	"time"
)

//使用：当有多个虚拟车时，同一时刻只有一个虚拟车与CANET关联

// BtmCanetManager BTM CANET 管理器
type BtmCanetManager interface {
	memory.VerifyEvn
	//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
	//最近一次车载ATP系统查询帧CRC16校验结果,true-校验通过
	atpReqCrc16Check bool
	//btm系统时间,每次接收到ATP查询请求帧时同步一次时间
	btmTime btmClock
	//数据流水号
	dsn byte
	//重发的数据
	resendData *resendData
}
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()
}

var (
	btmClientLocker sync.Mutex
	btmClient       BtmCanetClient
)

func Default() BtmCanetClient {
	btmClientLocker.Lock()
	defer btmClientLocker.Unlock()
	if btmClient == nil {
		btmClient = &btmCanetClient{}
	}
	return btmClient
}

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) {
	//一个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) {
	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
	se := fi.TrainBalisePowerAmplifierSwitch(s.bcm.EvnWorld(), atpReq.PowerAmplifierTurnOn)
	if se != nil {
		slog.Warn(fmt.Sprintf("列车车载BTM功率放大器开关控制异常[%s]", se.Error()))
	}
	//ATP 是否要求BTM 重发上一应答器报文
	isResendRequest := atpReq.ResendRequest == 2 //0b10
	s.rspToAtp(isResendRequest)
}

// BTM发送响应给ATP
// 当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
// 当未收到应答器报文时响应：时间同步帧、状态应答帧
func (s *btmCanetClient) rspToAtp(isResendRequest bool) {
	//重发上一报文处理
	if isResendRequest && s.resendData != nil && s.resendData.canResend() {
		s.resendData.countAdd1()
		s.sendCanetFrame(s.resendData.data)
		return
	} else {
		s.resendData = nil
	}
	//BTM状态
	statusF := message.NewBtmStatusRspFrame(s.atpReqSn)
	btmStatus, btmStatusErr := fi.FindTrainBaliseBtmStatus(s.bcm.EvnWorld())
	if btmStatusErr != nil {
		slog.Debug(fmt.Sprintf("从仿真获取BTM状态失败：%s", btmStatusErr.Error()))
		return
	}
	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()
	//
	baliseTelegram, btSendErr := fi.BaliseTelegramForSend(s.bcm.EvnWorld())
	if btSendErr != nil {
		//slog.Debug(btSendErr.Error())
	}
	//true-收到应答器报文
	isRcvTelegram := baliseTelegram != nil
	if isRcvTelegram { //当收到应答器报文时响应：时间同步帧、状态应答帧、数据帧
		statusDataCf, statusDataCfOk := message.CreateBtmRspFramesData(statusF, baliseTelegram.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()))
	}
}