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/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()
}

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) sendRadarInfo(ctx context.Context) {
	/*defer func() {
		slog.Error("")
	}()*/
	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}
			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
	}
}