rts-sim-testing-service/third_party/acc/acc_vobc.go

package acc

import (
	"context"
	"encoding/hex"
	"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 AccVobc interface {
	Start(accManager AccVobcManager)
	Stop()
	SendAcc(acc *message.Accelerometer)
	TrainAccSender(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState)
}

type AccVobcManager interface {
	GetRunAccConfig() config.AccConfig
	FindAccTrain() *state_proto.TrainState
}

var (
	initLock  = &sync.Mutex{}
	singleObj *accVobcService
)

func Default() AccVobc {
	defer initLock.Unlock()
	initLock.Lock()
	if singleObj == nil {
		singleObj = &accVobcService{}
	}
	return singleObj
}

type accVobcService struct {
	controlContext   context.CancelFunc
	vobcClient       udp.UdpClient
	radarVobcManager AccVobcManager
}

func (acc *accVobcService) Start(accManager AccVobcManager) {
	config := accManager.GetRunAccConfig()
	if config.RemoteIp == "" || config.RemotePort <= 0 || !config.Open {
		return
	}
	acc.vobcClient = udp.NewClient(fmt.Sprintf("%v:%v", config.RemoteIp, config.RemotePort))
	acc.radarVobcManager = accManager
}

func (avs *accVobcService) SendAcc(acc *message.Accelerometer) {
	if avs.vobcClient != nil {
		data := acc.Encode()
		hexStr := hex.EncodeToString(data)
		slog.Info("发送 加速度消息数据:", hexStr)
		err := avs.vobcClient.Send(data)
		if err != nil {
			slog.Info("发送 加速度消息数据失败:", hexStr)
			return
		}
	}
	//avs.vobcClient.Send(acc.BuildCanData())
}
func (avs *accVobcService) TrainAccSender(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState) {
	if trainState.VobcState.Tc1Active && trainState.TrainEndsA.AccEnable {
		s := parseAccSpeedData(info.Acceleration, trainState.TrainEndsA)
		avs.SendAcc(&message.Accelerometer{Acc: s})
	} else if trainState.VobcState.Tc2Active && trainState.TrainEndsB.AccEnable {
		s := parseAccSpeedData(info.Acceleration, trainState.TrainEndsB)
		avs.SendAcc(&message.Accelerometer{Acc: s})
	} else {
		avs.SendAcc(&message.Accelerometer{Acc: 0})
	}
}

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

	trainEndState.AccCheckTime = int32(trainEndState.AccCheckTimeOverAt - time.Now().Unix())
	if trainEndState.AccCheckTime <= 0 {
		//判断雷达检测时间是否到期
		trainEndState.AccCheckTime = 0
		if trainEndState.AccCheckTimeOverAt != 0 {
			trainEndState.AccCheckSpeedDiff = 0
			return accSpeed
		}
	}
	if trainEndState.AccCheckSpeedDiff > 0 {
		//如果雷达检测速度差值填写，那么就以速度差值为主
		return float32(math.Abs(float64(accSpeed - trainEndState.AccCheckSpeedDiff)))
	} else {
		return accSpeed
	}
}

/*func (acc *accVobcService) sendTask(ctx context.Context) {
	for {
		select {
		case <-ctx.Done():
			return
		default:
		}
		trainStatus := acc.radarVobcManager.FindAccTrain()
		if trainStatus != nil {
			// 发送加速度信息
			speedAcc := trainStatus.DynamicState.Acceleration
			t := speedAcc / accSpeedUnit
			acc.vobcClient.SendMsg(&message.Accelerometer{Acc: math.Float32bits(t)})
		}
		time.Sleep(time.Millisecond * accInterval)
	}
}*/

func (acc *accVobcService) Stop() {
	if acc.controlContext != nil {
		acc.controlContext()
		acc.controlContext = nil
	}
	if acc.vobcClient != nil {
		acc.vobcClient.Close()
		acc.vobcClient = nil
	}
}