101 lines
2.3 KiB
Go
101 lines
2.3 KiB
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
|
|
}
|
|
}
|