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/const/balise_const"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"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"
	"strings"
	"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
}

func (s *btmCanetClient) FindNotSendState() (state_proto.BTMState, bool) {
	s.baliseDetector.eqLock.Lock()
	defer s.baliseDetector.eqLock.Unlock()
	detector := s.baliseDetector
	for _, info := range detector.eq {
		if info != nil && !info.IsSend {
			info.IsSend = true
			return s.getStateFromDeta(info, uint32(detector.baliseCounter), uint32(detector.messageCounter), detector.aboveBalise), true

		}

	}
	return state_proto.BTMState{}, false
}

/*
	func (s *btmCanetClient) GetAllData() {
		detector := s.baliseDetector
		for _, info := range detector.eq {
			slog.Info("获取应答器信息 id:%v,是否发送:%v", info.BaliseId)
		}
	}
*/
func (s *btmCanetClient) getStateFromDeta(info *BtmAntennaScanningBaliseInfo, bc, mc uint32, aboveBalise bool) state_proto.BTMState {
	var telegram string
	var tel128 string
	var dis int64
	var baliseId string

	if /*detector.aboveBalise &&*/ info != nil && len(info.telegram) != 0 {
		telegram = fmt.Sprintf("%X", info.telegram)
		tel128 = fmt.Sprintf("%X", info.telegram128)
		dis = info.Distance
		baliseId = info.BaliseId
	} else {
		telegram = strings.Repeat("00", balise_const.UserTelegramByteLen)
		tel128 = strings.Repeat("00", balise_const.TelegramByteLen)
	}

	return state_proto.BTMState{
		BaliseId:         baliseId,
		DataSerialNumber: uint32(s.dsn),
		BaliseCount:      bc,
		MessageCounter:   mc,
		Telegram:         telegram,
		Telegram128:      tel128,
		Distance:         dis,
		AboveBalise:      aboveBalise,
	}
}
func (s *btmCanetClient) GetState() state_proto.BTMState {
	detector := s.baliseDetector
	info := detector.eq[len(detector.eq)-1]
	return s.getStateFromDeta(info, uint32(detector.baliseCounter), uint32(detector.messageCounter), detector.aboveBalise)

}

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, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo)
	//获取BTM显示状态 + btm最新的状态
	GetState() state_proto.BTMState
	//GetAllData()
	FindNotSendState() (state_proto.BTMState, bool)
}

var (
	btmClientLocker sync.Mutex
	btmClient       BtmCanetClient
)

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

// HandleTrainHeadPositionInfo 处理来自动力学的列车位置信息
func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo) {
	//slog.Debug(h.String())
	wd := entity.GetWorldData(w)
	repo := wd.Repo

	//s.baliseDetector.detect(wd, repo, h, vobcBtm)
	h2 := &TrainHeadPositionInfo{
		TrainId:      h.TrainId,
		Up:           h.Up,
		Link:         h.OldLink,
		LinkOffset:   h.OldLinkOffset,
		Speed:        h.Speed,
		Acceleration: h.Acceleration}
	s.baliseDetector.detect2(wd, repo, h, h2, vobcBtm)
}
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
	}
	for d := range s.baliseDetector.eq {
		s.baliseDetector.eq[d] = 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 = 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  //报文计数器（每解出一个应答器报文加一）
}