133 lines
3.6 KiB
Go
133 lines
3.6 KiB
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
|
||
}
|
||
}
|