rts-sim-testing-service/third_party/radar/radar_vobc.go

package radar

import (
	"context"
	"fmt"
	"joylink.club/bj-rtsts-server/config"
	"joylink.club/bj-rtsts-server/dto/common_proto"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"joylink.club/bj-rtsts-server/third_party/message"
	"joylink.club/bj-rtsts-server/third_party/udp"
	"log/slog"
	"math"
	"sync"
	"time"
)

type RadarVobc interface {
	Start(radar RadarVobcManager)
	Stop()
	SendMsg(ri *message.RadarInfo)
	TrainSpeedSender(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState)
}

type RadarVobcManager interface {
	GetRunRadarConfig() config.RadarConfig
	//FindRadarTrain() *state_proto.TrainState
}

const radar_interval = 15

var (
	initLock        = sync.Mutex{}
	_radar          *radarVobc
	fixed_speed     = 0.009155
	driftDefaultVal = 0.1
)

func Default() RadarVobc {
	defer initLock.Unlock()
	initLock.Lock()
	if _radar == nil {
		_radar = &radarVobc{}
	}
	return _radar
}

type radarVobc struct {
	radarVobcTaskContext context.CancelFunc
	vobcClient           udp.UdpClient
	radarVobcManager     RadarVobcManager
}

func (rv *radarVobc) Start(radar RadarVobcManager) {
	config := radar.GetRunRadarConfig()
	if config.RemoteIp == "" || config.RemotePort == 0 || !config.Open {
		slog.Info("雷达未开启", "远端ip:", config.RemoteIp, "远端端口:", config.RemotePort, "是否开启:", config.Open)
		return
	}

	rv.vobcClient = udp.NewClient(fmt.Sprintf("%v:%v", config.RemoteIp, config.RemotePort))
	//ctx, cancleFunc := context.WithCancel(context.Background())
	//rv.radarVobcTaskContext = cancleFunc
	rv.radarVobcManager = radar
	//go rv.sendRadarInfo(ctx)
}

// sendRadarInfo 发送速度，位移计数 给vobc
func (rv *radarVobc) SendMsg(ri *message.RadarInfo) {
	if rv.vobcClient != nil {
		rv.vobcClient.Send(ri.Encode())

	}
}
func (rv *radarVobc) TrainSpeedSender(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState) {
	forward := trainState.VobcState.DirectionForward
	if trainState.VobcState.Tc1Active && trainState.TrainEndsA.RadarEnable {
		s := parseRadarSpeedData(info.Speed, trainState.TrainEndsA)
		rv.SendMsg(message.NewRadarSender(s, forward, info.Displacement))
	} else if trainState.VobcState.Tc2Active && trainState.TrainEndsB.RadarEnable {
		s := parseRadarSpeedData(info.Speed, trainState.TrainEndsB)
		rv.SendMsg(message.NewRadarSender(s, forward, info.Displacement))
	} else {
		rv.SendMsg(message.NewRadarSender(0, forward, 0))
		//slog.Error("列车行驶方向不确定或两端的雷达均未启用,发送雷达数据0", "列车前进=", trainState.VobcState.DirectionForward, "雷达启用A=", trainState.TrainEndsA.RadarEnable, "列车后退=", trainState.VobcState.DirectionBackward, "雷达启用B=", trainState.TrainEndsB.RadarEnable)
	}
}

func parseRadarSpeedData(radarSpeed float32, trainEndState *common_proto.TrainEndsState) float32 {
	//如果差值速度和速度输出都填写，那么就以速度输出为优先
	//如果动力学速度-差值速度小于0，呢么输出的就是0（不能小于0）

	trainEndState.RadarCheckTime = int32(trainEndState.RadarCheckTimeOverAt - time.Now().Unix())
	if trainEndState.RadarCheckTime <= 0 {
		trainEndState.RadarCheckTime = 0
		if trainEndState.RadarCheckTimeOverAt != 0 {
			trainEndState.RadarCheckSpeedDiff = 0
			trainEndState.RadarOutSpeed = 0
			return radarSpeed
		}
	}

	if trainEndState.RadarCheckSpeedDiff > 0 && trainEndState.RadarOutSpeed > 0 {
		//如果雷达检测速度差值和速度输出都填写，那么就以速度输出为优先
		return float32(trainEndState.RadarOutSpeed)
	} else if trainEndState.RadarCheckSpeedDiff > 0 {
		//如果雷达检测速度差值填写，那么就以速度差值为主
		return float32(math.Abs(float64(radarSpeed - (trainEndState.RadarCheckSpeedDiff / 3.6))))
	} else if trainEndState.RadarOutSpeed > 0 {
		//如果雷达速度输出填写，那么就以速度输出为主
		return float32(trainEndState.RadarOutSpeed) / 3.6
	} else {
		return radarSpeed
	}
}

// sendRadarInfo 发送速度，位移计数 给vobc
/*
func (rv *radarVobc) sendRadarInfo(ctx context.Context) {

		for {
			select {
			case <-ctx.Done():
				return
			default:
			}
			trainStatus := rv.radarVobcManager.FindRadarTrain()
			if trainStatus != nil {
				hourSpeed := float64(trainStatus.DynamicState.Speed / 100)
				trainDift := trainStatus.DynamicState.Displacement
				td := float64(trainDift / 1000)
				s1 := uint16(math.Round(td / driftDefaultVal))
				s2 := uint16(math.Round(td / 1000 / driftDefaultVal))

				ri := message.RadarInfo{RealSpeed: uint16(math.Round(hourSpeed / fixed_speed)), DriftCounterS1: s1, DriftCounterS2: s2}
				ri.State = &message.RadarState{Dir: message.IsTrue(trainStatus.RunDirection)}
				rv.vobcClient.SendMsg(ri)
			}
			time.Sleep(time.Millisecond * radar_interval)
		}
	}
*/
func (rv *radarVobc) Stop() {

	/*	if rv.radarVobcTaskContext != nil {
		rv.radarVobcTaskContext()
		rv.radarVobcTaskContext = nil
	}*/
	if rv.vobcClient != nil {
		rv.vobcClient.Close()
		rv.vobcClient = nil
	}
}