列车控制修复“测速测距”失败问题调整
This commit is contained in:
tiger_zhou
parent
bedb2bf413
commit
6a0a47083f
56
third_party/message/train_pc_sim_message.go
vendored
56
third_party/message/train_pc_sim_message.go
vendored
@ -4,7 +4,6 @@ import (
|
||||
"bytes"
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"math"
|
||||
"strconv"
|
||||
"time"
|
||||
)
|
||||
@ -122,66 +121,21 @@ func AtpLowPowerByte(d byte) bool {
|
||||
|
||||
// 列车速度位置报告
|
||||
type TrainSpeedPlaceReportMsg struct {
|
||||
PulseCount1 float32
|
||||
PulseCount2 float32
|
||||
Pp1 float32
|
||||
Pp2 float32
|
||||
HistoryCount float32
|
||||
Tmp bool
|
||||
PcCount float32
|
||||
PulseCount1 float32
|
||||
PulseCount2 float32
|
||||
}
|
||||
|
||||
func (tp *TrainSpeedPlaceReportMsg) ParsePulseCount1(s1, s2 uint32) {
|
||||
//tp.PulseCount1 += tp.HistoryCount
|
||||
//tp.PulseCount2 += tp.HistoryCount
|
||||
|
||||
tp.PulseCount1 += float32(s1) * 200 / math.Pi / 840
|
||||
tp.PulseCount2 += float32(s2) * 200 / math.Pi / 840
|
||||
//tp.HistoryCount += s1
|
||||
}
|
||||
func (tp *TrainSpeedPlaceReportMsg) ParsePulseCut(s1, s2 float32) {
|
||||
/* if s1 == 0 || s2 == 0 {
|
||||
tp.PulseCount1 = tp.HistoryCount
|
||||
tp.PulseCount2 = tp.HistoryCount
|
||||
return
|
||||
}*/
|
||||
tp.HistoryCount += s1
|
||||
d1 := tp.PulseCount1 - s1
|
||||
if d1 <= 0 {
|
||||
d1 = 0
|
||||
}
|
||||
d2 := tp.PulseCount1 - s2
|
||||
if d2 <= 0 {
|
||||
d2 = 0
|
||||
}
|
||||
tp.PulseCount1 = d1
|
||||
tp.PulseCount2 = d2
|
||||
}
|
||||
|
||||
func (tp *TrainSpeedPlaceReportMsg) Encode(runDir uint16, s1 uint32) []byte {
|
||||
func (tp *TrainSpeedPlaceReportMsg) Encode(runDir uint16, s1, runRange uint32) []byte {
|
||||
data := make([]byte, 0)
|
||||
|
||||
data = binary.BigEndian.AppendUint16(data, runDir)
|
||||
|
||||
data = binary.BigEndian.AppendUint32(data, s1)
|
||||
data = binary.BigEndian.AppendUint32(data, s1)
|
||||
tp.PcCount = tp.PulseCount1 * 200 / math.Pi / float32(840)
|
||||
|
||||
data = binary.BigEndian.AppendUint32(data, uint32(tp.PcCount))
|
||||
data = binary.BigEndian.AppendUint32(data, uint32(tp.PcCount))
|
||||
data = binary.BigEndian.AppendUint32(data, runRange)
|
||||
data = binary.BigEndian.AppendUint32(data, runRange)
|
||||
|
||||
/*now := time.Now().UTC()
|
||||
// 将时间转换为毫秒
|
||||
millis := now.UnixNano() / int64(time.Millisecond)
|
||||
millisStr := strconv.Itoa(int(millis))
|
||||
strs := []rune(millisStr)
|
||||
sec := now.Unix()
|
||||
|
||||
millisStr2 := strconv.Itoa(int(sec))
|
||||
strs2 := []rune(millisStr2)
|
||||
dd, _ := strconv.Atoi(string(strs2[5:]))
|
||||
|
||||
mm, _ := strconv.Atoi(string(strs[len(strs)-3:]))*/
|
||||
sec, ms := tp.time()
|
||||
data = binary.BigEndian.AppendUint32(data, sec)
|
||||
data = binary.BigEndian.AppendUint16(data, ms)
|
||||
|
||||
50
third_party/train_pc_sim/train_pc_sim.go
vendored
50
third_party/train_pc_sim/train_pc_sim.go
vendored
@ -48,8 +48,7 @@ type TrainPcSim interface {
|
||||
CreateOrRemoveTrain(train *state_proto.TrainState, isCreate bool) error
|
||||
// TrainPluseCount 计算列车脉冲
|
||||
TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32)
|
||||
//ResetPluseCount(sta *state_proto.TrainState)
|
||||
//FindAllThirdPartState() []tpapi.ThirdPartyApiService
|
||||
|
||||
DsnAddAndReturn() byte
|
||||
}
|
||||
|
||||
@ -74,7 +73,8 @@ type TrainPcSimManage interface {
|
||||
type trainPcSimService struct {
|
||||
state tpapi.ThirdPartyApiServiceState
|
||||
newPcSimclientMap map[string]*TrainPcReciverData
|
||||
cancleContext context.CancelFunc
|
||||
cancleContextFun context.CancelFunc
|
||||
context context.Context
|
||||
trainPcSimManage TrainPcSimManage
|
||||
configs []config.VehiclePCSimConfig
|
||||
btmDsn uint8
|
||||
@ -140,8 +140,8 @@ func (d *trainPcSimService) findAllThirdPartState() []tpapi.ThirdPartyApiService
|
||||
// 速度(单位mm/s)对应的脉冲数：速度*200/pi/840；
|
||||
// 里程（单位mm）对应的脉冲：总里程*200/pi/840
|
||||
func pluseCountSpeed(wheelDiameter int32, speedMeter float32) uint32 {
|
||||
s1 := speedMeter * 1000
|
||||
pluseCountData := s1 * 200 / math.Pi / float32(wheelDiameter)
|
||||
|
||||
pluseCountData := speedMeter * 200 / math.Pi / float32(wheelDiameter)
|
||||
return uint32(pluseCountData)
|
||||
}
|
||||
|
||||
@ -158,12 +158,18 @@ func (d *trainPcSimService) pluseSpeed(sta *state_proto.TrainState) (uint32, flo
|
||||
}
|
||||
d.TrainPluseCountReset(sta)
|
||||
speed := sum / float32(pcLen)
|
||||
return pluseCountSpeed(sta.WheelDiameter, speed), speed
|
||||
return pluseCountSpeed(sta.WheelDiameter, speed*1000), speed
|
||||
|
||||
}
|
||||
func (d *trainPcSimService) TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32) {
|
||||
defer initLock.Unlock()
|
||||
initLock.Lock()
|
||||
|
||||
select {
|
||||
case <-d.context.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
if sd, err := d.findTrainConn(sta); err == nil {
|
||||
sd.speedPlace.PulseCount1 += sta.DynamicState.Displacement
|
||||
sd.speedPlace.PulseCount2 = sd.speedPlace.PulseCount1
|
||||
@ -279,7 +285,8 @@ func (d *trainPcSimService) Start(pcSimManage TrainPcSimManage) {
|
||||
|
||||
d.configs = configs
|
||||
ctx, ctxFun := context.WithCancel(context.Background())
|
||||
d.cancleContext = ctxFun
|
||||
d.cancleContextFun = ctxFun
|
||||
d.context = ctx
|
||||
d.trainPcSimManage = pcSimManage
|
||||
go d.sendTrainLocationAndSpeedTask(ctx)
|
||||
}
|
||||
@ -288,9 +295,9 @@ func (d *trainPcSimService) Stop() {
|
||||
for _, data := range d.newPcSimclientMap {
|
||||
data.updateState(tpapi.ThirdPartyState_Closed)
|
||||
}
|
||||
if d.cancleContext != nil {
|
||||
d.cancleContext()
|
||||
d.cancleContext = nil
|
||||
if d.cancleContextFun != nil {
|
||||
d.cancleContextFun()
|
||||
d.cancleContextFun = nil
|
||||
}
|
||||
d.newCloseAllConn()
|
||||
}
|
||||
@ -322,13 +329,13 @@ func (d *trainPcSimService) CreateOrRemoveTrain(train *state_proto.TrainState, i
|
||||
func (d *trainPcSimService) initTrain(rd *TrainPcReciverData, train *state_proto.TrainState, isCreate bool, trains *message.TrainPcSimBaseMessage) error {
|
||||
msgs := make([]message.TrainPcSimBaseMessage, 0)
|
||||
sendMsg := make([]byte, 0)
|
||||
rd.speedPlace = &message.TrainSpeedPlaceReportMsg{}
|
||||
train.PluseCount = &state_proto.SensorSpeedPulseCount{}
|
||||
rd.train = train
|
||||
tcc := train.Tcc
|
||||
tcc.LineInitTimeStamp12 = 0
|
||||
tcc.Line12ConnErr = false
|
||||
if isCreate {
|
||||
rd.speedPlace = &message.TrainSpeedPlaceReportMsg{}
|
||||
train.PluseCount = &state_proto.SensorSpeedPulseCount{}
|
||||
rd.train = train
|
||||
tcc := train.Tcc
|
||||
tcc.LineInitTimeStamp12 = 0
|
||||
tcc.Line12ConnErr = false
|
||||
tmpMsgs := d.trainPcSimManage.ObtainTrainDigitalMockData(train)
|
||||
msgs = append(msgs, tmpMsgs...)
|
||||
msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{0x00}, Type: message.RECIVE_TRAIN_DOOR_MODE}) //门模式
|
||||
@ -336,7 +343,7 @@ func (d *trainPcSimService) initTrain(rd *TrainPcReciverData, train *state_proto
|
||||
} else {
|
||||
train.VobcState.Tc1Active = false
|
||||
train.VobcState.Tc2Active = false
|
||||
tcc := train.Tcc
|
||||
|
||||
for _, key := range tcc.DriverKey {
|
||||
key.Val = false
|
||||
}
|
||||
@ -377,17 +384,16 @@ func (d *trainPcSimService) sendTrainLocationAndSpeedTask(ctx context.Context) {
|
||||
connState = tpapi.ThirdPartyState_Broken
|
||||
}
|
||||
trainClient.updateState(connState)
|
||||
s1, speed := d.pluseSpeed(train)
|
||||
|
||||
s1, _ := d.pluseSpeed(train)
|
||||
runDir := uint16(2)
|
||||
if train.Tcc.DirKey.Val == 1 {
|
||||
runDir = 1
|
||||
}
|
||||
data := trainClient.speedPlace.Encode(runDir, s1)
|
||||
disPluse := pluseCountSpeed(train.WheelDiameter, trainClient.speedPlace.PulseCount1)
|
||||
data := trainClient.speedPlace.Encode(runDir, s1, disPluse)
|
||||
bm := &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_LOCATION_INFO, Data: data}
|
||||
|
||||
dataCode := bm.Encode()
|
||||
slog.Info(fmt.Sprintf("发送列车速度位置,列车：%v,列车速度:%v,计数脉冲: %v,累计里程: %v ,累计脉冲:%v,发送数据:%v", train.Id, speed, s1, trainClient.speedPlace.PulseCount1, trainClient.speedPlace.PcCount, hex.EncodeToString(dataCode)))
|
||||
//slog.Info(fmt.Sprintf("发送列车速度位置,列车：%v,列车速度:%v,计数脉冲: %v,累计里程: %v ,发送数据:%v", train.Id, speed, s1, trainClient.speedPlace.PulseCount1, hex.EncodeToString(dataCode)))
|
||||
err := trainClient.tcpClient.Send(dataCode)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("发送列车速度位置失败,列车：%v,发送数据:%v", train.Id, hex.EncodeToString(dataCode)))
|
||||
|
||||
Loading…
Reference in New Issue
Block a user