rts-sim-testing-service/third_party/electrical_machinery/electrical_machinery.go

package electrical_machinery

import (
	"encoding/hex"
	"fmt"
	"joylink.club/bj-rtsts-server/dto/common_proto"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"log/slog"
	"sync"

	"joylink.club/bj-rtsts-server/config"
	"joylink.club/bj-rtsts-server/third_party/message"
	"joylink.club/bj-rtsts-server/third_party/udp"
)

const (
	PointA = iota + 1
	PointB
)

// 电机转速UDP
type ElectricMachinery interface {
	Start(manager ElectricMachineryMessageManager)                                                     // 启动电机转速UDP消息处理
	Stop()                                                                                             // 停止电机转速消息处理
	SendElectricMachineryMessage(emMap map[int]*message.ElectricMachinery)                             // 发送电机转速消息
	SendElectricMachineryMessage2(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState) // 发送电机转速消息
	ClearOrRemoveTrain(trainState *state_proto.TrainState)
}

type ElectricMachineryMessageManager interface {
	GetElectricMachineryRunConfig() []config.ElectricMachineryConfig // 获取电机转速参数
}

type electricalMachineryImpl struct {
	//electricalMachineryUdpClient udp.UdpClient

	electricalMachineryUdpClientMap map[int]udp.UdpClient
	manager                         ElectricMachineryMessageManager
	runConfig                       []config.ElectricMachineryConfig
}

func (s *electricalMachineryImpl) Start(manager ElectricMachineryMessageManager) {
	if manager == nil {
		panic("启动电机转速消息服务错误: ElectricMachineryMessageManager不能为nil")
	}
	if s.manager != nil {
		panic("启动电机转速消息服务错误: 存在正在运行的任务")
	}
	s.runConfig = manager.GetElectricMachineryRunConfig()
	allNotOpen := true
	for _, c := range s.runConfig {
		if &c != nil && c.Open {
			allNotOpen = false
		}
	}
	if allNotOpen {
		return
	}
	// 初始化客户端、服务端
	s.initElectricalMachinery()
	s.manager = manager
}

func (s *electricalMachineryImpl) Stop() {
	initMutex.Lock()
	defer initMutex.Unlock()
	_default = nil
	//if s.electricalMachineryUdpClient != nil {
	//	s.electricalMachineryUdpClient.Close()
	//}
	for _, c := range s.electricalMachineryUdpClientMap {
		if &c != nil {
			c.Close()
		}
	}
	s.manager = nil
}
func (s *electricalMachineryImpl) ClearOrRemoveTrain(trainState *state_proto.TrainState) {
	collectSpeedMap := initEMMsg(trainState)
	if trainState.VobcState.Tc1Active {
		collectSpeed(0, 0, trainState.TrainEndsA, collectSpeedMap)

	} else if trainState.VobcState.Tc2Active {
		collectSpeed(0, 0, trainState.TrainEndsB, collectSpeedMap)
	}
	// 更新电机转速
	s.SendElectricMachineryMessage(collectSpeedMap)
}
func (s *electricalMachineryImpl) SendElectricMachineryMessage(emMap map[int]*message.ElectricMachinery) {
	for key, em := range emMap {
		client := s.electricalMachineryUdpClientMap[key]
		if client != nil {
			data := em.Encode()
			hexStr := hex.EncodeToString(data)
			slog.Info("发送电机转速消息key", hexStr)
			err := client.Send(data)
			if err != nil {
				slog.Info("发送电机转速消息失败key", hexStr)
			}
		}
	}
}
func (s *electricalMachineryImpl) SendElectricMachineryMessage2(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState) {
	collectSpeedMap := initEMMsg(trainState)
	if trainState.VobcState.Tc1Active {
		collectSpeed(info.HeadSpeed1, info.HeadSpeed2, trainState.TrainEndsA, collectSpeedMap)

	} else if trainState.VobcState.Tc2Active {
		collectSpeed(info.TailSpeed1, info.TailSpeed2, trainState.TrainEndsB, collectSpeedMap)
	}
	// 更新电机转速
	s.SendElectricMachineryMessage(collectSpeedMap)
}
func collectSpeed(speed1, speed2 float32, endP *common_proto.TrainEndsState, resultMap map[int]*message.ElectricMachinery) {
	pa := resultMap[PointA]
	pb := resultMap[PointB]
	if endP.SpeedSensorEnableA || endP.SpeedSensorEnableB {
		if endP.AccOutSpeed > 0 {
			pa.Speed = float32(endP.AccOutSpeed) / 3.6
			pb.Speed = float32(endP.AccOutSpeed) / 3.6
		} else {
			if endP.SpeedSensorEnableA {
				pa.Speed = speed1
			}
			if endP.SpeedSensorEnableB {
				pb.Speed = speed2
			}
		}
	}
}

func initEMMsg(trainState *state_proto.TrainState) map[int]*message.ElectricMachinery {

	collectSpeedMap := map[int]*message.ElectricMachinery{
		PointA: {
			Speed:         0,
			WheelDiameter: trainState.WheelDiameter,
			IsBack:        trainState.VobcState.DirectionBackward},
		PointB: {Speed: 0,
			WheelDiameter: trainState.WheelDiameter,
			IsBack:        trainState.VobcState.DirectionBackward},
	}
	return collectSpeedMap
}

func (s *electricalMachineryImpl) initElectricalMachinery() {
	s.electricalMachineryUdpClientMap = map[int]udp.UdpClient{}
	for _, c := range s.runConfig {
		if &c != nil && c.Ip != "" && c.Open {
			ep := PointA
			if !c.EndPointA {
				ep = PointB
			}
			s.electricalMachineryUdpClientMap[ep] = udp.NewClient(fmt.Sprintf("%v:%v", c.Ip, c.RemotePort))
		}
	}
}

var _default ElectricMachinery
var initMutex sync.Mutex

func Default() ElectricMachinery {
	initMutex.Lock()
	defer initMutex.Unlock()
	if _default == nil {
		_default = &electricalMachineryImpl{}
	}
	return _default
}