列车11号线btm调整
This commit is contained in:
tiger_zhou
parent
cf3d38a2f5
commit
f485e7a639
@ -96,6 +96,7 @@ func createByProjectId(c *gin.Context) {
|
||||
if len(mapInfos) == 0 {
|
||||
panic(sys_error.New("测试启动失败,项目未关联发布图"))
|
||||
}
|
||||
|
||||
var mapIds []int32
|
||||
for _, mapInfo := range mapInfos {
|
||||
if mapInfo.Type == data_proto.PictureType_value[data_proto.PictureType_TrainData.String()] {
|
||||
@ -251,6 +252,7 @@ func addTrain(c *gin.Context) {
|
||||
TrainMaxAcc: req.TrainMaxAcc,
|
||||
TrainMaxBrake: req.TrainMaxBrake,
|
||||
TrainEmergencyBrake: req.TrainEmergencyBrake,
|
||||
ProjectCode: simulation.ProjectCode,
|
||||
//HeadOffset: 93211,
|
||||
DevicePort: req.DevicePort,
|
||||
TrainRunUp: req.RunDirection,
|
||||
|
||||
@ -152,8 +152,8 @@ func trainPcConnErr(err error) {
|
||||
func main() {
|
||||
|
||||
initConfig()
|
||||
//initBtmTest()
|
||||
initTrainPc()
|
||||
initBtmTest()
|
||||
//initTrainPc()
|
||||
//initAccTest()
|
||||
//initSpeedTest()
|
||||
|
||||
@ -168,9 +168,9 @@ func sendPacket(lifeNum uint32, autoId byte) {
|
||||
userMsg, _ := hex.DecodeString(testUserBtmMsg)
|
||||
|
||||
msg := &message.BtmVobcMessage{FontTtl: 2, BtmStatus: 0x00, DecodeTime: 10, BackTtl: 2, BtmMsg: userMsg, ResponseTime: 10,
|
||||
VobcLifeNum: lifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoId}, MsgSerial: message.GetAutoMessageId()}
|
||||
VobcLifeNum: lifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoId} /*MsgSerial: message.GetAutoMessageId()*/}
|
||||
sendData := msg.Encode()
|
||||
fmt.Println("发送btm vobc len:", len(sendData), "报文:", hex.EncodeToString(sendData), "报文序列号:", msg.MsgSerial)
|
||||
//fmt.Println("发送btm vobc len:", len(sendData), "报文:", hex.EncodeToString(sendData), "报文序列号:", msg.MsgSerial)
|
||||
time.Sleep(time.Millisecond * 100)
|
||||
err := btmCli.Send(sendData)
|
||||
if err != nil {
|
||||
@ -194,9 +194,9 @@ func sendPacketFree(lifeNum uint32, autoId byte, msgs byte) {
|
||||
}
|
||||
msg2 := &message.BtmVobcMsgFree{BtmStatus: 0x00, WorkTemperature: 10, Fun1: uint16(0), Fun2: uint16(0), Fun3: uint16(0), Fun4: uint16(0),
|
||||
//FreeMsg: freeMsg, RespTime: 20, VobcLifeNum: lifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoId}, MsgSerial: newMsg}
|
||||
FreeMsg: freeMsg, RespTime: 20, VobcLifeNum: lifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoId}, MsgSerial: message.GetAutoMessageId()}
|
||||
FreeMsg: freeMsg, RespTime: 20, VobcLifeNum: lifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoId} /* MsgSerial: message.GetAutoMessageId()*/}
|
||||
sendData2 := msg2.Encode()
|
||||
fmt.Println("发送btm vobc 空报文:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", autoId, "报文序列号:", msg2.MsgSerial)
|
||||
//fmt.Println("发送btm vobc 空报文:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", autoId, "报文序列号:", msg2.MsgSerial)
|
||||
time.Sleep(time.Millisecond * 100)
|
||||
err := btmCli.Send(sendData2)
|
||||
if err != nil {
|
||||
@ -207,15 +207,15 @@ func sendPacketFree(lifeNum uint32, autoId byte, msgs byte) {
|
||||
func RequestFramePackets(req *message.BtmVobcReq, vobcLife uint32) {
|
||||
//fmt.Println(fmt.Sprintf("接受 请求帧 frameStatus:%v,messageType:%v,lifeNum:%v,序列号:%v", req.FrameStatus, req.MessageType, req.VobcLifeNum, req.MessageSerial))
|
||||
if req.FrameStatus == message.REQ_FRAME_STATUS_BOOT && req.MessageType == message.REQ_PACKETS_TYPE_BOOT {
|
||||
fmt.Println("000000000000000000000000000")
|
||||
//fmt.Println("000000000000000000000000000")
|
||||
sendPacketFree(vobcLife, req.AutoIdFrame, req.MessageSerial)
|
||||
} else if req.FrameStatus == message.REQ_FRAME_STATUS_OK {
|
||||
//帧正确，删除之前发送的数据
|
||||
fmt.Println("11111111111111111111")
|
||||
//fmt.Println("11111111111111111111")
|
||||
|
||||
} else if req.FrameStatus == message.REQ_FRAME_STATUS_ERROR {
|
||||
//帧不正确 重新发送2次，如果2次后仍然不正确，则删除之前发送的数据
|
||||
fmt.Println("22222222222222222")
|
||||
///**/fmt.Println("22222222222222222")
|
||||
}
|
||||
}
|
||||
|
||||
@ -224,7 +224,7 @@ var vobcNumLife uint32 = 0
|
||||
|
||||
func handleBtmVobcFrames(cfs []byte) {
|
||||
|
||||
fmt.Println(fmt.Sprintf("收到源数据:%v ,请求帧时间:%v ,vobcLife:%v", hex.EncodeToString(cfs), time.Now().UnixMilli()-btmReceiveTime, vobcNumLife))
|
||||
fmt.Println(fmt.Sprintf("收到源数据:%v ,数据长度:%v", hex.EncodeToString(cfs), len(cfs)))
|
||||
frameType, dataText, err := message.BtmVobcDecode(cfs)
|
||||
if err != nil {
|
||||
return
|
||||
@ -241,11 +241,11 @@ func handleBtmVobcFrames(cfs []byte) {
|
||||
if vobcNumLife <= 0 {
|
||||
return
|
||||
} else {
|
||||
fmt.Println(fmt.Sprintf("准备发送vobcLife:%v", vobcNumLife))
|
||||
//fmt.Println(fmt.Sprintf("准备发送vobcLife:%v", vobcNumLife))
|
||||
}
|
||||
req := &message.BtmVobcReq{}
|
||||
req.Decode(dataText)
|
||||
fmt.Println(fmt.Sprintf("接受 请求帧 frameStatus:%v,messageType:%v,lifeNum:%v,序列号:%v", req.FrameStatus, req.MessageType, req.VobcLifeNum, req.MessageSerial))
|
||||
//fmt.Println(fmt.Sprintf("接受 请求帧 frameStatus:%v,messageType:%v,lifeNum:%v,序列号:%v", req.FrameStatus, req.MessageType, req.VobcLifeNum, req.MessageSerial))
|
||||
if time.Now().UnixMilli()-btmReceiveTime > 20*1000 {
|
||||
idCommand := &message.BtmVobcIdCommand{}
|
||||
idCommand.Decode(dataText)
|
||||
|
||||
@ -6,8 +6,8 @@ server:
|
||||
# 数据源
|
||||
datasource:
|
||||
# 数据库访问url
|
||||
# dsn: root:root@tcp(127.0.0.1:3306)/bj-rtss?charset=utf8mb4&parseTime=true&loc=UTC
|
||||
dsn: root:joylink0503@tcp(192.168.33.233:3306)/bj-rtss?charset=utf8mb4&parseTime=true&loc=UTC
|
||||
dsn: root:root@tcp(127.0.0.1:3306)/bj-rtss?charset=utf8mb4&parseTime=true&loc=UTC
|
||||
# dsn: root:joylink0503@tcp(192.168.33.233:3306)/bj-rtss?charset=utf8mb4&parseTime=true&loc=UTC
|
||||
|
||||
# 日志配置
|
||||
logging:
|
||||
@ -33,7 +33,7 @@ logging:
|
||||
# 消息配置
|
||||
messaging:
|
||||
mqtt:
|
||||
# address: tcp://127.0.0.1:1883
|
||||
address: tcp://192.168.33.233:1883
|
||||
address: tcp://127.0.0.1:1883
|
||||
# address: tcp://192.168.33.233:1883
|
||||
username: rtsts_service
|
||||
password: joylink@0503
|
||||
File diff suppressed because it is too large
Load Diff
@ -528,7 +528,7 @@ func convertTrainState(v *state_proto.TrainState) *state_proto.TrainMapState {
|
||||
TailDeviceId: v.TailDeviceId,
|
||||
TailOffset: v.TailOffset,
|
||||
TailDevicePort: v.TailDevicePort,
|
||||
BtmState: v.BtmState,
|
||||
//BtmState: v.BtmState,
|
||||
ConnState: v.ConnState,
|
||||
}
|
||||
convertDynamicConfig(v.TrainDynamicConfig, t.TrainDynamicConfig)
|
||||
|
||||
@ -1 +1 @@
|
||||
Subproject commit a94fdb12f720a5d9bbf57b1916e8ba5990130e6e
|
||||
Subproject commit 693576459c8875f7fd82c84df1cd39d4487be65d
|
||||
@ -1 +1 @@
|
||||
Subproject commit 7629f290ea612d4ce368c40d4cf793dffc4b21ec
|
||||
Subproject commit 3d7da35731be5af7bb126c276b4f9486c1f68e3a
|
||||
378
third_party/btm_vobc/btm_vobc.go
vendored
378
third_party/btm_vobc/btm_vobc.go
vendored
@ -1,378 +0,0 @@
|
||||
package btm_vobc
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/hex"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"github.com/google/uuid"
|
||||
"github.com/snksoft/crc"
|
||||
"joylink.club/bj-rtsts-server/config"
|
||||
"joylink.club/bj-rtsts-server/const/balise_const"
|
||||
"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"
|
||||
"reflect"
|
||||
"strconv"
|
||||
"time"
|
||||
|
||||
"strings"
|
||||
"sync"
|
||||
)
|
||||
|
||||
type BtmVobcManage interface {
|
||||
GetBtmVobcConfig() config.BtmVobcConfig
|
||||
|
||||
GetAllTrain() []*state_proto.TrainState
|
||||
GetConnVobcTrain() *state_proto.TrainState
|
||||
}
|
||||
type BtmVobcService interface {
|
||||
Start(btmVobcManage BtmVobcManage)
|
||||
Stop()
|
||||
SendData(data []byte)
|
||||
AppendBaliseMsgForTrain(vobcBtm *state_proto.VobcBtmState, baliseId string, baliseSource []byte, arriveTime int64)
|
||||
UpdateTrainLeave(vobcBtm *state_proto.VobcBtmState, baliseId string, leaveTime int64)
|
||||
RemoveBaliseFirst(vobcBtm *state_proto.VobcBtmState)
|
||||
}
|
||||
type BtmVobcClient struct {
|
||||
calFun context.CancelFunc
|
||||
client udp.UdpClient
|
||||
server udp.UdpServer
|
||||
manage BtmVobcManage
|
||||
}
|
||||
|
||||
var (
|
||||
btmVobcLocker sync.Mutex
|
||||
btmVobcClient *BtmVobcClient
|
||||
btmVobcBaliseLocker sync.Mutex
|
||||
//最新接受数据时间
|
||||
reviceTimeStamp int64
|
||||
)
|
||||
|
||||
func Default() BtmVobcService {
|
||||
defer btmVobcLocker.Unlock()
|
||||
btmVobcLocker.Lock()
|
||||
if btmVobcClient == nil {
|
||||
btmVobcClient = &BtmVobcClient{}
|
||||
}
|
||||
return btmVobcClient
|
||||
}
|
||||
|
||||
// 缓存列车经过的应答器报文
|
||||
func (b *BtmVobcClient) AppendBaliseMsgForTrain(vobcBtm *state_proto.VobcBtmState, baliseId string, baliseSource []byte, arriveTime int64) {
|
||||
defer btmVobcBaliseLocker.Unlock()
|
||||
btmVobcBaliseLocker.Lock()
|
||||
baliseHex := hex.EncodeToString(baliseSource)
|
||||
tel := &state_proto.VobcBtmState_TelegramState{Telegram: baliseHex, BaliseId: baliseId, ArriveTime: arriveTime}
|
||||
if len(vobcBtm.TelegramState) == 0 {
|
||||
vobcBtm.TelegramState = append(vobcBtm.TelegramState, tel)
|
||||
} else {
|
||||
bs := crc.CalculateCRC(crc.CRC32, baliseSource)
|
||||
exists := false
|
||||
for _, d := range vobcBtm.TelegramState {
|
||||
sd, _ := hex.DecodeString(d.Telegram)
|
||||
t := crc.CalculateCRC(crc.CRC32, sd)
|
||||
if t == bs {
|
||||
exists = true
|
||||
}
|
||||
}
|
||||
if !exists {
|
||||
if len(vobcBtm.TelegramState) >= 8 {
|
||||
vobcBtm.TelegramState = append(vobcBtm.TelegramState[1:], tel)
|
||||
} else {
|
||||
vobcBtm.TelegramState = append(vobcBtm.TelegramState, tel)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
func (b *BtmVobcClient) UpdateTrainLeave(vobcBtm *state_proto.VobcBtmState, baliseId string, leaveTime int64) {
|
||||
defer btmVobcBaliseLocker.Unlock()
|
||||
btmVobcBaliseLocker.Lock()
|
||||
if vobcBtm.TelegramState == nil || len(vobcBtm.TelegramState) == 0 {
|
||||
return
|
||||
}
|
||||
for _, bs := range vobcBtm.TelegramState {
|
||||
if bs.BaliseId == baliseId {
|
||||
bs.LeaveTime = leaveTime
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) RemoveBaliseFirst(vobcBtm *state_proto.VobcBtmState) {
|
||||
defer btmVobcBaliseLocker.Unlock()
|
||||
btmVobcBaliseLocker.Lock()
|
||||
|
||||
if vobcBtm.TelegramState == nil || len(vobcBtm.TelegramState) == 0 {
|
||||
return
|
||||
}
|
||||
vobcBtm.TelegramState = vobcBtm.TelegramState[1:]
|
||||
}
|
||||
func (b *BtmVobcClient) Start(btmVobcManage BtmVobcManage) {
|
||||
cfg := btmVobcManage.GetBtmVobcConfig()
|
||||
if !cfg.Open {
|
||||
slog.Info("11号线 btm vobc配置未开启...")
|
||||
return
|
||||
}
|
||||
udpServer := udp.NewServer(fmt.Sprintf("%v:%d", cfg.LocalUdpIp, cfg.LocalUdpPort), b.handleBtmVobcFrames)
|
||||
err := udpServer.Listen()
|
||||
if err != nil {
|
||||
slog.Error("11号线 btm VOBC 服务启动失败...")
|
||||
return
|
||||
}
|
||||
//
|
||||
udpClient := udp.NewClient(fmt.Sprintf("%s:%d", cfg.RemoteIp, cfg.RemoteUdpPort))
|
||||
b.manage = btmVobcManage
|
||||
b.server = udpServer
|
||||
b.client = udpClient
|
||||
reviceTimeStamp = time.Now().UnixMilli()
|
||||
ctx, calFun := context.WithCancel(context.Background())
|
||||
b.calFun = calFun
|
||||
go b.checkTrainTTLIsOver(ctx)
|
||||
}
|
||||
func (b *BtmVobcClient) checkTrainTTLIsOver(ctx context.Context) {
|
||||
defer btmVobcBaliseLocker.Unlock()
|
||||
btmVobcBaliseLocker.Lock()
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
return
|
||||
default:
|
||||
}
|
||||
if time.Now().UnixMilli()-reviceTimeStamp < 1000 {
|
||||
return
|
||||
}
|
||||
trains := b.manage.GetAllTrain()
|
||||
if len(trains) > 0 {
|
||||
slog.Info("检测网络中断情况 前沿ttl 超时...")
|
||||
newTel := make([]*state_proto.VobcBtmState_TelegramState, 0)
|
||||
for _, train := range b.manage.GetAllTrain() {
|
||||
for _, state := range train.VobcBtm.TelegramState {
|
||||
if time.Now().UnixMilli()-state.ArriveTime < 1000 {
|
||||
newTel = append(newTel, state)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
time.Sleep(time.Second * 1)
|
||||
}
|
||||
|
||||
}
|
||||
func (b *BtmVobcClient) handleBtmVobcFrames(cfs []byte) {
|
||||
reviceTimeStamp = time.Now().UnixMilli()
|
||||
|
||||
train := b.manage.GetConnVobcTrain()
|
||||
if train == nil {
|
||||
//slog.Error("vobc btm 未找到VOBC连接的列车...")
|
||||
return
|
||||
}
|
||||
requestId := uuid.New().String()
|
||||
slog.Info(fmt.Sprintf("获取到vobc btm原始数据:%v, requestId:%v", hex.EncodeToString(cfs), requestId))
|
||||
frameType, dataText, err := message.BtmVobcDecode(cfs)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("%v,请求id:%v", err, requestId))
|
||||
return
|
||||
}
|
||||
receiveDataTime := time.Now().UnixMicro()
|
||||
decodePayMicoTime := (receiveDataTime - receiveDataTime) / 100
|
||||
if frameType == message.COMMAND_TYPE {
|
||||
idCommand := &message.BtmVobcIdCommand{}
|
||||
idCommand.Decode(dataText)
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的id命令帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
|
||||
b.packets(requestId, idCommand.VobcLifeNum, idCommand.AutoIdFrame, receiveDataTime, decodePayMicoTime, train.VobcBtm)
|
||||
|
||||
} else if frameType == message.REQUEST_TYPE {
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的请求帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
|
||||
req := &message.BtmVobcReq{}
|
||||
req.Decode(dataText)
|
||||
b.RequestFramePackets(req.VobcLifeNum, req.AutoIdFrame, requestId, receiveDataTime, decodePayMicoTime, req, train.VobcBtm)
|
||||
} else {
|
||||
slog.Error(fmt.Sprintf("btm vobc 解析未知命令帧类型:0x%v,原始数据:%v,长度:%v,requestId:%v", strconv.FormatInt(int64(frameType), 16), hex.EncodeToString(cfs), len(cfs), requestId))
|
||||
return
|
||||
}
|
||||
}
|
||||
func createFreeBalisePacketString() string {
|
||||
return strings.Repeat("00", balise_const.UserTelegramByteLen)
|
||||
}
|
||||
func createFreeBalisePackets() []byte {
|
||||
str := createFreeBalisePacketString()
|
||||
d, _ := hex.DecodeString(str)
|
||||
return d
|
||||
}
|
||||
|
||||
const MAX_SEND_COUNT = 3
|
||||
|
||||
// 请求帧
|
||||
func (b *BtmVobcClient) RequestFramePackets(vobcLifeNum uint32, autoId byte, requestId string, receiveTime int64, decodePayMicoTime int64, req *message.BtmVobcReq, vobcbtm *state_proto.VobcBtmState) {
|
||||
if req.FrameStatus == message.REQ_FRAME_STATUS_BOOT && req.MessageType == message.REQ_PACKETS_TYPE_BOOT {
|
||||
slog.Info(fmt.Sprintf("接受请求帧，准备发送空闲帧数据 帧状态:0x%v,消息类型:0x%v ,requestId:%v", strconv.FormatInt(int64(req.FrameStatus), 16), strconv.FormatInt(int64(req.MessageType), 16), requestId))
|
||||
vobcbtm.TelegramState = make([]*state_proto.VobcBtmState_TelegramState, 0)
|
||||
vobcbtm.History = make(map[uint32]*state_proto.VobcBtmState_VobcBtmHistoryState)
|
||||
b.packets(requestId, vobcLifeNum, autoId, receiveTime, decodePayMicoTime, vobcbtm)
|
||||
|
||||
} else if req.FrameStatus == message.REQ_FRAME_STATUS_OK {
|
||||
//帧正确，删除之前发送的数据
|
||||
slog.Info(fmt.Sprintf("接受请求帧，帧正确，删除之前发送的数据requestId:%v", requestId))
|
||||
delete(vobcbtm.History, uint32(req.MessageSerial))
|
||||
} else if req.FrameStatus == message.REQ_FRAME_STATUS_ERROR {
|
||||
//帧不正确 重新发送2次，如果2次后仍然不正确，则删除之前发送的数据
|
||||
dd := vobcbtm.History[uint32(req.MessageSerial)]
|
||||
if dd.SendCount > MAX_SEND_COUNT {
|
||||
slog.Info(fmt.Sprintf("接受请求帧，帧不正确，重发超过2次，删除之前发送的数据,准备发送新数据 requestId:%v", requestId))
|
||||
sendBalisePacket := dd.BalisePacket
|
||||
delete(vobcbtm.History, uint32(req.MessageSerial))
|
||||
if vobcbtm.TelegramState[0].Telegram == sendBalisePacket {
|
||||
b.RemoveBaliseFirst(vobcbtm)
|
||||
}
|
||||
if len(vobcbtm.TelegramState) == 0 {
|
||||
b.balisePacketsFree(requestId, time.Now().UnixMicro(), req.VobcLifeNum, req.AutoIdFrame, vobcbtm)
|
||||
} else {
|
||||
b.balisePackets(requestId, vobcbtm.TelegramState[0], receiveTime, decodePayMicoTime, req.VobcLifeNum, req.AutoIdFrame, vobcbtm)
|
||||
}
|
||||
|
||||
} else {
|
||||
dd.SendCount = dd.SendCount + 1
|
||||
if dd.IsFreePacket {
|
||||
freeMsg := &message.BtmVobcMsgFree{}
|
||||
if b.unmarshalJson(dd.SendTelegram, freeMsg) == nil {
|
||||
freeMsg.FreeMsg = createFreeBalisePackets()
|
||||
repeatData := freeMsg.Encode()
|
||||
logStr := fmt.Sprintf("重新发送空闲帧数据，发送次数:%v,帧系列id:%v,数据:%v,requestId:%v", dd.SendCount, freeMsg.MsgSerial, hex.EncodeToString(repeatData), requestId)
|
||||
slog.Info(logStr)
|
||||
err := b.client.Send(repeatData)
|
||||
if err != nil {
|
||||
slog.Error(logStr, err)
|
||||
|
||||
}
|
||||
}
|
||||
} else {
|
||||
packetMsg := &message.BtmVobcMessage{}
|
||||
if b.unmarshalJson(dd.SendTelegram, packetMsg) == nil {
|
||||
balise, _ := hex.DecodeString(dd.BalisePacket)
|
||||
packetMsg.BtmMsg = balise
|
||||
baliseData := packetMsg.Encode()
|
||||
logStr := fmt.Sprintf("重新发送空闲帧数据，发送次数:%v,帧系列id:%v 数据:%v ,requestId:%v", dd.SendCount, packetMsg.MsgSerial, hex.EncodeToString(baliseData), requestId)
|
||||
slog.Info(logStr)
|
||||
err := b.client.Send(baliseData)
|
||||
if err != nil {
|
||||
slog.Error(logStr, err)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) unmarshalJson(jsonStr string, obj any) error {
|
||||
err := json.Unmarshal([]byte(jsonStr), obj)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("vobc btm 数据转换失败 source:%v,对象:%v", jsonStr, reflect.TypeOf(obj).Name()))
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// 有应答器报文
|
||||
func (b *BtmVobcClient) balisePackets(requestId string, tel *state_proto.VobcBtmState_TelegramState, receiveTime int64, decodePayMicoTime int64, vobcLifeNum uint32, autoCommandId byte, vobcbtm *state_proto.VobcBtmState) {
|
||||
|
||||
data, e := hex.DecodeString(tel.Telegram)
|
||||
if e != nil {
|
||||
slog.Error(fmt.Sprintf("解析应答器报文失败应答器报文长度:%v", tel.Telegram), e)
|
||||
return
|
||||
}
|
||||
if len(data) < balise_const.UserTelegramByteLen {
|
||||
for i := 0; i < balise_const.UserTelegramByteLen-len(data); i++ {
|
||||
data = append(data, 0)
|
||||
}
|
||||
}
|
||||
//前沿时间
|
||||
|
||||
var fttl uint16 = 0
|
||||
tmpFttl := int64(math.Abs(float64(time.Now().UnixMilli() - tel.ArriveTime)))
|
||||
if tmpFttl >= 0xffff {
|
||||
fttl = 0xffff
|
||||
} else {
|
||||
fttl = uint16(tmpFttl)
|
||||
}
|
||||
|
||||
var bttl uint16 = 0
|
||||
tmpBttl := int64(math.Abs(float64(time.Now().UnixMilli() - tel.ArriveTime)))
|
||||
if tmpBttl >= 0xffff {
|
||||
bttl = 0xffff
|
||||
} else {
|
||||
bttl = uint16(tmpBttl)
|
||||
}
|
||||
repTimeMicro := (receiveTime - time.Now().UnixMicro()) / 100
|
||||
baliseMsg := &message.BtmVobcMessage{FontTtl: fttl, BtmStatus: message.BTM_STSTUS_NORMAL, DecodeTime: uint16(decodePayMicoTime),
|
||||
BackTtl: bttl, ResponseTime: byte(repTimeMicro), MsgSerial: message.GetAutoMessageId(),
|
||||
VobcLifeNum: vobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoCommandId}}
|
||||
u32MsgId := uint32(baliseMsg.MsgSerial)
|
||||
jsonArr, _ := json.Marshal(baliseMsg)
|
||||
baliseMsg.BtmMsg = data
|
||||
baliseData := baliseMsg.Encode()
|
||||
baliseMsgHex := hex.EncodeToString(baliseData)
|
||||
vobcbtm.History[u32MsgId] = &state_proto.VobcBtmState_VobcBtmHistoryState{SendCount: 1, PacketSendId: u32MsgId, VobcLifeNum: vobcLifeNum,
|
||||
IsFreePacket: false, SendTelegram: string(jsonArr), BalisePacket: tel.Telegram}
|
||||
logStr := fmt.Sprintf("发送btm vobc 报文数据 报文序列id:%v 报文内容:%v 长度:%v ,requestId:%v", u32MsgId, baliseMsgHex, len(baliseData), requestId)
|
||||
slog.Info(logStr)
|
||||
err := b.client.Send(baliseData)
|
||||
if err != nil {
|
||||
slog.Error(logStr, err)
|
||||
return
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
// 无应答器报文
|
||||
func (b *BtmVobcClient) balisePacketsFree(requestId string, receiveTime int64, vobcLifeNum uint32, autoCommandId byte, vobcbtm *state_proto.VobcBtmState) {
|
||||
repTimeMicro := (receiveTime - time.Now().UnixMicro()) / 100
|
||||
data := createFreeBalisePackets()
|
||||
freeMsg := &message.BtmVobcMsgFree{BtmStatus: message.BTM_STSTUS_NORMAL, Fun1: 0xffff, Fun2: 0x00CF, Fun3: uint16(0), Fun4: uint16(0),
|
||||
FreeMsg: data, RespTime: byte(repTimeMicro), MsgSerial: message.GetAutoMessageId(), VobcLifeNum: vobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoCommandId}}
|
||||
u32MsgId := uint32(freeMsg.MsgSerial)
|
||||
jsonArr, _ := json.Marshal(freeMsg)
|
||||
vobcbtm.History[u32MsgId] = &state_proto.VobcBtmState_VobcBtmHistoryState{SendCount: 1, PacketSendId: u32MsgId, VobcLifeNum: vobcLifeNum, IsFreePacket: true, SendTelegram: string(jsonArr)}
|
||||
freeData := freeMsg.Encode()
|
||||
dataEncode := hex.EncodeToString(freeData)
|
||||
logStr := fmt.Sprintf("发送btm vobc 报文序列id:%v 空闲帧报文:%v 长度:%v ,requestId:%v", u32MsgId, dataEncode, len(freeData), requestId)
|
||||
slog.Info(logStr)
|
||||
err := b.client.Send(freeData)
|
||||
if err != nil {
|
||||
slog.Error(logStr, err)
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
// 应答器报文或空报文
|
||||
func (b *BtmVobcClient) packets(requestId string, vobcLifeNum uint32, autoIdFrame byte, receiveTime int64, decodePayMicoTime int64, vobcbtm *state_proto.VobcBtmState) {
|
||||
|
||||
if len(vobcbtm.TelegramState) == 0 {
|
||||
b.balisePacketsFree(requestId, receiveTime, vobcLifeNum, autoIdFrame, vobcbtm)
|
||||
} else {
|
||||
b.balisePackets(requestId, vobcbtm.TelegramState[0], receiveTime, decodePayMicoTime, vobcLifeNum, autoIdFrame, vobcbtm)
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) SendData(data []byte) {
|
||||
if b.client != nil {
|
||||
slog.Info(fmt.Sprintf("发送btm vobc 报文:%v 长度:%v", hex.EncodeToString(data), len(data)))
|
||||
err := b.client.Send(data)
|
||||
if err != nil {
|
||||
slog.Error("发送btm vobc 报文失败:", err)
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
func (b *BtmVobcClient) Stop() {
|
||||
if b.server != nil {
|
||||
b.calFun()
|
||||
b.server.Close()
|
||||
}
|
||||
if b.client != nil {
|
||||
b.client.Close()
|
||||
}
|
||||
}
|
||||
44
third_party/can_btm/balise_btm.go
vendored
44
third_party/can_btm/balise_btm.go
vendored
@ -53,39 +53,6 @@ type btmCanetClient struct {
|
||||
baliseDetector *BaliseDetector
|
||||
}
|
||||
|
||||
func (s *btmCanetClient) FindBaliseResend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte) {
|
||||
s.baliseDetector.eqLock.Lock()
|
||||
defer s.baliseDetector.eqLock.Unlock()
|
||||
cache := train.BtmBaliseCache
|
||||
|
||||
for _, balise := range cache.BaliseList {
|
||||
slog.Info(fmt.Sprintf("===========%v", len(cache.BaliseList)))
|
||||
if balise != nil && balise.BaliseId == cache.ResendBaliseId && balise.ResendCount < 3 {
|
||||
balise.ResendCount++
|
||||
ndsn := BaliseCounterAdd(cache.Dsn)
|
||||
cache.Dsn = uint32(ndsn)
|
||||
return balise, ndsn, byte(cache.BaliseCount), byte(cache.MessageCounter)
|
||||
}
|
||||
}
|
||||
return nil, 0, 0, 0
|
||||
}
|
||||
|
||||
func (s *btmCanetClient) FindBaliseByNotSend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte) {
|
||||
s.baliseDetector.eqLock.Lock()
|
||||
defer s.baliseDetector.eqLock.Unlock()
|
||||
cache := train.BtmBaliseCache
|
||||
for _, btmCache := range cache.BaliseList {
|
||||
if btmCache != nil && !btmCache.IsSend {
|
||||
ndsn := BaliseCounterAdd(cache.Dsn)
|
||||
cache.Dsn = uint32(ndsn)
|
||||
cache.ResendBaliseId = btmCache.BaliseId
|
||||
return btmCache, ndsn, byte(cache.BaliseCount), byte(cache.MessageCounter)
|
||||
}
|
||||
}
|
||||
|
||||
return nil, 0, 0, 0
|
||||
}
|
||||
|
||||
type BtmClock struct {
|
||||
BtmTk uint32 //与ATP系统同步的时间ms
|
||||
SysTk time.Time //本地系统时间
|
||||
@ -100,11 +67,8 @@ type BtmCanetClient interface {
|
||||
Start(bcm BtmCanetManager)
|
||||
Stop()
|
||||
//HandleTrainHeadPositionInfo 处理收到列车位置信息
|
||||
HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo)
|
||||
HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo)
|
||||
HandleTrainHeadPositionInfoForTrain(w ecs.World, train *state_proto.TrainBtmCache, h *TrainHeadPositionInfo)
|
||||
|
||||
FindBaliseByNotSend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte)
|
||||
FindBaliseResend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte)
|
||||
}
|
||||
|
||||
var (
|
||||
@ -130,16 +94,16 @@ func (s *btmCanetClient) HandleTrainHeadPositionInfoForTrain(w ecs.World, btmCac
|
||||
LinkOffset: h.OldLinkOffset,
|
||||
Speed: h.Speed,
|
||||
Acceleration: h.Acceleration}
|
||||
s.baliseDetector.newDetect(wd, repo, h, h2, btmCache)
|
||||
s.baliseDetector.newDetect(wd, repo, h, h2, btmCache, h.IsLine12)
|
||||
//s.baliseDetector.detect2(wd, repo, h, h2, vobcBtm)
|
||||
}
|
||||
|
||||
// HandleTrainHeadPositionInfo 处理来自动力学的列车位置信息
|
||||
func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo) {
|
||||
func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo) {
|
||||
//slog.Debug(h.String())
|
||||
wd := entity.GetWorldData(w)
|
||||
repo := wd.Repo
|
||||
s.baliseDetector.detect(wd, repo, h, vobcBtm)
|
||||
s.baliseDetector.detect(wd, repo, h)
|
||||
}
|
||||
func (s *btmCanetClient) Start(bcm BtmCanetManager) {
|
||||
s.bcm = bcm
|
||||
|
||||
77
third_party/can_btm/balise_detection.go
vendored
77
third_party/can_btm/balise_detection.go
vendored
@ -2,9 +2,7 @@ package can_btm
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"joylink.club/bj-rtsts-server/const/balise_const"
|
||||
"joylink.club/bj-rtsts-server/dto/state_proto"
|
||||
"joylink.club/bj-rtsts-server/third_party/btm_vobc"
|
||||
"joylink.club/rtsssimulation/component"
|
||||
"joylink.club/rtsssimulation/fi"
|
||||
"joylink.club/rtsssimulation/repository"
|
||||
@ -12,7 +10,6 @@ import (
|
||||
"log/slog"
|
||||
"math"
|
||||
"sort"
|
||||
"strings"
|
||||
"sync"
|
||||
"time"
|
||||
)
|
||||
@ -80,7 +77,7 @@ func (t *BaliseDetector) tryRebind(th *TrainHeadPositionInfo) {
|
||||
//slog.Debug(fmt.Sprintf("列车[%s]与CAN-BTM绑定", t.trianId))
|
||||
}
|
||||
}
|
||||
func (t *BaliseDetector) newDetect(wd *component.WorldData, repo *repository.Repository, th, th2 *TrainHeadPositionInfo, btmCache *state_proto.TrainBtmCache) {
|
||||
func (t *BaliseDetector) newDetect(wd *component.WorldData, repo *repository.Repository, th, th2 *TrainHeadPositionInfo, btmCache *state_proto.TrainBtmCache, isLine12 bool) {
|
||||
//BTM天线中心点运行信息
|
||||
curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th) //目前车头
|
||||
curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, th2) //上次车头
|
||||
@ -90,13 +87,8 @@ func (t *BaliseDetector) newDetect(wd *component.WorldData, repo *repository.Rep
|
||||
} else {
|
||||
startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)
|
||||
}
|
||||
|
||||
//startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi.LinkId, th2.LinkOffset, th.LinkOffset)
|
||||
//startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)
|
||||
|
||||
balises := make([]*repository.Transponder, 0)
|
||||
for _, balise := range startBalises {
|
||||
//slog.Info(fmt.Sprintf("id:%v , %v,linkeId:%v ,headoffset:%v,tailOffset:%v", balise.Id(), th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset))
|
||||
find := false
|
||||
for _, transponder := range balises {
|
||||
if transponder.Id() == balise.Id() {
|
||||
@ -110,16 +102,13 @@ func (t *BaliseDetector) newDetect(wd *component.WorldData, repo *repository.Rep
|
||||
}
|
||||
if len(balises) > 0 {
|
||||
balise := balises[0]
|
||||
//slog.Info(fmt.Sprintf("-----------------id:%v ,offset:%v, up: %v,linkeId:%v ,headoffset:%v,tailOffset:%v", balise.Id(), balise.LinkPosition().Offset(), th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset))
|
||||
telegram, utel := t.rcvTelegram(wd, balise.Id())
|
||||
if t.addNewExpectedBalise(balise, btmCache, telegram, utel) {
|
||||
//slog.Info(fmt.Sprintf("+++++++++++++id:%v ,offset:%v, up: %v,linkeId:%v ,headoffset:%v,tailOffset:%v", balise.Id(), balise.LinkPosition().Offset(), th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset))
|
||||
}
|
||||
AddNewExpectedBalise(balise, btmCache, telegram, utel, isLine12)
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
|
||||
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
|
||||
t.tryRebind(th)
|
||||
//if !t.powerAmplifierSwitch { //天线功率放大器未开启，不进行探测
|
||||
// return
|
||||
@ -131,14 +120,9 @@ func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Reposi
|
||||
curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)
|
||||
|
||||
if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms
|
||||
//if curExpect != nil && curExpect.Distance < 80 { //20ms
|
||||
//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
|
||||
//slog.Info(fmt.Sprintf("baliseId:%v,Distance:%v,up:%v", curExpect.BaliseId, curExpect.Distance, curAntennaRi.Up))
|
||||
|
||||
telegram, utel := t.rcvTelegram(wd, curExpect.BaliseId)
|
||||
|
||||
if curExpect.Distance <= 50 {
|
||||
btm_vobc.Default().AppendBaliseMsgForTrain(vobcBtm, curExpect.BaliseId, telegram, curTime.UnixMilli())
|
||||
}
|
||||
//记录即将经过的应答器
|
||||
if t.addExpectedBalise(curExpect) {
|
||||
t.baliseCounterAdd1() //应答器计数器
|
||||
@ -154,16 +138,6 @@ func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Reposi
|
||||
t.aboveBalise = false
|
||||
}
|
||||
|
||||
curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, &TrainHeadPositionInfo{TrainId: th.TrainId,
|
||||
Up: !th.Up,
|
||||
Link: th.Link,
|
||||
LinkOffset: th.LinkOffset,
|
||||
Speed: th.Speed,
|
||||
Acceleration: th.Acceleration})
|
||||
curExpect2 := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi2)
|
||||
if curExpect2 != nil && curExpect2.Distance > 20 {
|
||||
btm_vobc.Default().UpdateTrainLeave(vobcBtm, curExpect2.BaliseId, curTime.UnixMilli())
|
||||
}
|
||||
}
|
||||
|
||||
// 应答器计数器加1，[0,255]
|
||||
@ -182,12 +156,15 @@ func (t *BaliseDetector) baliseMessageCounterAdd1() {
|
||||
}
|
||||
}
|
||||
|
||||
func BaliseCounterAdd(counter uint32) byte {
|
||||
func BaliseCounterAdd(counter uint32, isLine12 bool) byte {
|
||||
c := byte(counter)
|
||||
c++
|
||||
if c > 255 {
|
||||
c = 1
|
||||
if isLine12 {
|
||||
c = 0
|
||||
}
|
||||
}
|
||||
return c
|
||||
}
|
||||
|
||||
@ -217,44 +194,6 @@ func (t *BaliseDetector) rcvTelegram(wd *component.WorldData, baliseId string) (
|
||||
}
|
||||
return nil, nil
|
||||
}
|
||||
func (t *BaliseDetector) addNewExpectedBalise(balise *repository.Transponder, btmCache *state_proto.TrainBtmCache, telegram, userTelegram []byte) bool {
|
||||
t.eqLock.Lock()
|
||||
defer t.eqLock.Unlock()
|
||||
bl := btmCache.BaliseList
|
||||
for _, tt := range bl {
|
||||
if tt != nil && tt.BaliseId == balise.Id() {
|
||||
return false
|
||||
}
|
||||
}
|
||||
for i := 1; i < len(bl); i++ {
|
||||
bl[i-1] = bl[i]
|
||||
}
|
||||
unpack := false
|
||||
bc := BaliseCounterAdd(btmCache.BaliseCount)
|
||||
mc := btmCache.MessageCounter
|
||||
if userTelegram != nil && len(userTelegram) > 0 {
|
||||
mc = uint32(BaliseCounterAdd(mc))
|
||||
unpack = true
|
||||
}
|
||||
|
||||
btmCache.BaliseCount = uint32(bc)
|
||||
btmCache.MessageCounter = mc
|
||||
btmS := &state_proto.BTMState{BaliseId: balise.Id(),
|
||||
Telegram: fmt.Sprintf("%x", userTelegram),
|
||||
Telegram128: fmt.Sprintf("%X", telegram),
|
||||
Unpack: unpack,
|
||||
BaliseType: int32(balise.BaliseType().Number()),
|
||||
AboveBalise: true, HasData: true}
|
||||
|
||||
if userTelegram == nil || len(userTelegram) == 0 {
|
||||
btmS.Telegram = strings.Repeat("00", balise_const.UserTelegramByteLen)
|
||||
btmS.Telegram128 = strings.Repeat("00", balise_const.TelegramByteLen)
|
||||
btmS.HasData = false
|
||||
}
|
||||
//存入队尾
|
||||
bl[len(bl)-1] = btmS
|
||||
return true
|
||||
}
|
||||
|
||||
// true-新增；false-更新
|
||||
func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseInfo) bool {
|
||||
|
||||
121
third_party/can_btm/train_balise_cache.go
vendored
Normal file
121
third_party/can_btm/train_balise_cache.go
vendored
Normal file
@ -0,0 +1,121 @@
|
||||
package can_btm
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"joylink.club/bj-rtsts-server/const/balise_const"
|
||||
"joylink.club/bj-rtsts-server/dto/state_proto"
|
||||
"joylink.club/rtsssimulation/repository"
|
||||
"strings"
|
||||
"sync"
|
||||
)
|
||||
|
||||
var baliseLock = &sync.Mutex{}
|
||||
|
||||
func IsLine12(train *state_proto.TrainState) bool {
|
||||
if strings.Contains(train.ProjectCode, "12") {
|
||||
return true
|
||||
}
|
||||
return false
|
||||
}
|
||||
func AddNewExpectedBalise(balise *repository.Transponder, btmCache *state_proto.TrainBtmCache, telegram, userTelegram []byte, isLine12 bool) bool {
|
||||
baliseLock.Lock()
|
||||
defer baliseLock.Unlock()
|
||||
bl := btmCache.BaliseList
|
||||
for _, tt := range bl {
|
||||
if tt != nil && tt.BaliseId == balise.Id() {
|
||||
return false
|
||||
}
|
||||
}
|
||||
for i := 1; i < len(bl); i++ {
|
||||
bl[i-1] = bl[i]
|
||||
}
|
||||
unpack := false
|
||||
bc := BaliseCounterAdd(btmCache.BaliseCount, isLine12)
|
||||
mc := btmCache.MessageCounter
|
||||
if userTelegram != nil && len(userTelegram) > 0 {
|
||||
mc = uint32(BaliseCounterAdd(mc, isLine12))
|
||||
unpack = true
|
||||
}
|
||||
|
||||
btmCache.BaliseCount = uint32(bc)
|
||||
btmCache.MessageCounter = mc
|
||||
btmS := &state_proto.BTMState{BaliseId: balise.Id(),
|
||||
Telegram: fmt.Sprintf("%x", userTelegram),
|
||||
Telegram128: fmt.Sprintf("%X", telegram),
|
||||
Unpack: unpack,
|
||||
BaliseType: int32(balise.BaliseType().Number()),
|
||||
AboveBalise: true, HasData: true}
|
||||
|
||||
if userTelegram == nil || len(userTelegram) == 0 {
|
||||
btmS.Telegram = strings.Repeat("00", balise_const.UserTelegramByteLen)
|
||||
btmS.Telegram128 = strings.Repeat("00", balise_const.TelegramByteLen)
|
||||
btmS.HasData = false
|
||||
}
|
||||
//存入队尾
|
||||
bl[len(bl)-1] = btmS
|
||||
return true
|
||||
}
|
||||
|
||||
// HandleTrainHeadPositionInfoForTrain 处理列车位置信息
|
||||
// 参数1： 参数2：发送序列号，参数3：应答器计数（每过一个应答器加一，在同一个应答器内不变)，参数4：报文计数器 (每解出一个报文加一)（0~255）
|
||||
func FindBaliseResend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte) {
|
||||
baliseLock.Lock()
|
||||
defer baliseLock.Unlock()
|
||||
cache := train.BtmBaliseCache
|
||||
for _, balise := range cache.BaliseList {
|
||||
if balise != nil && balise.BaliseId == cache.ResendBaliseId && balise.ResendCount < 3 {
|
||||
balise.ResendCount++
|
||||
ndsn := BaliseCounterAdd(cache.Dsn, IsLine12(train))
|
||||
cache.Dsn = uint32(ndsn)
|
||||
return balise, ndsn, byte(cache.BaliseCount), byte(cache.MessageCounter)
|
||||
}
|
||||
}
|
||||
ndsn := BaliseCounterAdd(cache.Dsn, IsLine12(train))
|
||||
return nil, ndsn, 0, 0
|
||||
}
|
||||
func FindBaliseByNotSend(train *state_proto.TrainState) (*state_proto.BTMState, byte, byte, byte) {
|
||||
baliseLock.Lock()
|
||||
defer baliseLock.Unlock()
|
||||
cache := train.BtmBaliseCache
|
||||
for _, btmCache := range cache.BaliseList {
|
||||
if btmCache != nil && !btmCache.IsSend {
|
||||
ndsn := BaliseCounterAdd(cache.Dsn, IsLine12(train))
|
||||
cache.Dsn = uint32(ndsn)
|
||||
cache.ResendBaliseId = btmCache.BaliseId
|
||||
return btmCache, ndsn, byte(cache.BaliseCount), byte(cache.MessageCounter)
|
||||
}
|
||||
}
|
||||
ndsn := BaliseCounterAdd(cache.Dsn, IsLine12(train))
|
||||
return nil, ndsn, 0, 0
|
||||
}
|
||||
func ClearBalise(train *state_proto.TrainState) {
|
||||
baliseLock.Lock()
|
||||
defer baliseLock.Unlock()
|
||||
train.BtmBaliseCache.BaliseList = make([]*state_proto.BTMState, 3)
|
||||
}
|
||||
|
||||
// 11号线根据序列号查询
|
||||
func FindBaliseByMessageSerial(train *state_proto.TrainState, ms byte) (*state_proto.BTMState, byte, bool) {
|
||||
baliseLock.Lock()
|
||||
defer baliseLock.Unlock()
|
||||
cache := train.BtmBaliseCache
|
||||
|
||||
for _, btmCache := range cache.BaliseList {
|
||||
if btmCache != nil {
|
||||
if btmCache.BaliseId == cache.ResendBaliseId {
|
||||
if byte(btmCache.PackageDataSN) == ms {
|
||||
bt, dsn, _, _ := FindBaliseByNotSend(train)
|
||||
return bt, dsn, true
|
||||
} else {
|
||||
ndsn := BaliseCounterAdd(cache.Dsn, IsLine12(train))
|
||||
cache.Dsn = uint32(ndsn)
|
||||
return btmCache, ndsn, false
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bt, dsn, _, _ := FindBaliseByNotSend(train)
|
||||
return bt, dsn, true
|
||||
|
||||
}
|
||||
468
third_party/message/btm_vobc_data.go
vendored
468
third_party/message/btm_vobc_data.go
vendored
@ -75,355 +75,47 @@ const (
|
||||
COM_WAIT_STOP_FD = 0x03
|
||||
)
|
||||
|
||||
func aa(src []byte, dest []byte) uint16 {
|
||||
var (
|
||||
SrcPos, TgtPos, Pos1, Pos2, iii, Gap uint16
|
||||
Got1stFF uint8
|
||||
)
|
||||
srouceLen := uint16(len(src))
|
||||
for SrcPos = 0; SrcPos < srouceLen; SrcPos++ {
|
||||
if Got1stFF == 1 {
|
||||
if src[SrcPos] == 0xff {
|
||||
//Got2ndFF = 1
|
||||
Pos2 = SrcPos
|
||||
dest[TgtPos] = byte(Pos2 - Pos1)
|
||||
TgtPos++
|
||||
/*
|
||||
func SourceDataDecode(packData []byte) []BaseBtmVobc {
|
||||
data, _ := TranslateFromFFFE(packData)
|
||||
buf := bytes.NewBuffer(data)
|
||||
|
||||
for iii = Pos1 + 1; iii < Pos2; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
Got1stFF = 0
|
||||
//Got2ndFF = 0
|
||||
Pos1 = 0
|
||||
Pos2 = 0
|
||||
Gap = 0
|
||||
} else {
|
||||
/*已遇到前一个FF，且当前遇到非FF*/
|
||||
Gap++
|
||||
if 252 == Gap {
|
||||
Got1stFF = 0
|
||||
Gap = 0
|
||||
dest[TgtPos] = 0
|
||||
TgtPos++
|
||||
for iii = Pos1 + 1; iii <= SrcPos; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/*尚未遇到前一个FF*/
|
||||
dest[TgtPos] = src[SrcPos]
|
||||
TgtPos++
|
||||
if 0xFF == src[SrcPos] {
|
||||
/*遇到前一个FF*/
|
||||
Got1stFF = 1
|
||||
Pos1 = SrcPos
|
||||
Gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
if 1 == Got1stFF {
|
||||
dest[TgtPos] = 0
|
||||
TgtPos++
|
||||
for iii = Pos1 + 1; iii < srouceLen; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
return TgtPos
|
||||
}
|
||||
|
||||
func TranslateFromFFFE(pSrc []byte) ([]byte, uint16) {
|
||||
var (
|
||||
srcPos, tgtPos, nonFFCount, nextFFPos uint16
|
||||
char uint8
|
||||
state int
|
||||
)
|
||||
func parseData(buf *bytes.Buffer) {
|
||||
h1, _ := buf.ReadByte()
|
||||
h2, _ := buf.ReadByte()
|
||||
if bytes.Equal(PACKAGE_HEAD, []byte{h1, h2}) {
|
||||
t, _ := buf.ReadByte()
|
||||
var ft byte = 0
|
||||
if t == ID_COMMAND_TYPE || t == REQT_TYPE {
|
||||
ft, _ = buf.ReadByte()
|
||||
if t == ID_COMMAND_TYPE {
|
||||
return parseIdFrame(ft, buf.Bytes())
|
||||
} else if t == REQT_TYPE {
|
||||
|
||||
if len(pSrc) == 0 {
|
||||
return nil, 0 // 入参错误
|
||||
}
|
||||
|
||||
pTgt := make([]byte, len(pSrc)) // 预分配与输入等长的缓冲区
|
||||
tgtPos = 0
|
||||
state = WAIT_FF_C1
|
||||
nonFFCount = 0
|
||||
|
||||
for srcPos = 0; srcPos < uint16(len(pSrc)); srcPos++ {
|
||||
char = pSrc[srcPos]
|
||||
switch state {
|
||||
case WAIT_FF_C1:
|
||||
if char == 0xFF {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
state = WAIT_FF_C2
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
}
|
||||
case WAIT_FF_C2:
|
||||
if char == 0xFD || char == 0xFE || char == 0xFF {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
if char == 0 {
|
||||
state = WAIT_NO_FF
|
||||
} else if char == 1 {
|
||||
pTgt[tgtPos] = 0xFF
|
||||
tgtPos++
|
||||
state = WAIT_FF_C1
|
||||
} else {
|
||||
nextFFPos = srcPos + uint16(char) - 1
|
||||
state = WAIT_FF_POS
|
||||
}
|
||||
}
|
||||
case WAIT_NO_FF:
|
||||
nonFFCount++
|
||||
if char == 0xFF && nonFFCount < 252 {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
if nonFFCount == 252 {
|
||||
nonFFCount = 0
|
||||
state = WAIT_FF_C1
|
||||
}
|
||||
}
|
||||
case WAIT_FF_POS:
|
||||
if char == 0xFF {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
if srcPos == nextFFPos {
|
||||
pTgt[tgtPos] = 0xFF
|
||||
tgtPos++
|
||||
state = WAIT_FF_C1
|
||||
}
|
||||
}
|
||||
default:
|
||||
state = ABNORMAL_STATE
|
||||
}
|
||||
|
||||
if state == ABNORMAL_STATE {
|
||||
tgtPos = 0
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
// 退出时的状态判断
|
||||
if state == WAIT_FF_C2 || state == WAIT_FF_POS {
|
||||
tgtPos = 0
|
||||
}
|
||||
|
||||
return pTgt[:tgtPos], tgtPos
|
||||
}
|
||||
|
||||
// TranslateToFFFE 对给定的字节切片进行FFFE转义处理
|
||||
func TranslateToFFFE(src []byte) ([]byte, error) {
|
||||
if src == nil {
|
||||
return nil, fmt.Errorf("source data is nil")
|
||||
}
|
||||
var tgt []byte
|
||||
var pos1, pos2 int
|
||||
var gap int
|
||||
var got1stFF bool
|
||||
|
||||
for i, b := range src {
|
||||
if got1stFF {
|
||||
if b == 0xFF {
|
||||
// 已遇到前一个FF，且当前又遇到FF
|
||||
got1stFF = false
|
||||
pos2 = i
|
||||
if gap > 252 {
|
||||
// 间隙过大，特殊处理
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:pos2]...)
|
||||
} else {
|
||||
// 写入间隙长度
|
||||
tgt = append(tgt, byte(gap))
|
||||
// 写入间隙中的字节
|
||||
tgt = append(tgt, src[pos1+1:pos2]...)
|
||||
}
|
||||
} else {
|
||||
// 已遇到前一个FF，且当前遇到非FF，增加gap计数
|
||||
gap++
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
tgt = append(tgt, b)
|
||||
if b == 0xFF {
|
||||
// 遇到前一个FF
|
||||
got1stFF = true
|
||||
pos1 = i
|
||||
gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果在数据末尾遇到了FF且没有第二个FF
|
||||
if got1stFF {
|
||||
if gap > 252 {
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:]...)
|
||||
} else {
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:len(src)]...)
|
||||
}
|
||||
}
|
||||
|
||||
return tgt, nil
|
||||
}
|
||||
|
||||
// TranslateToFFFE 将数据进行FFFE转义处理（不加头FFFE及尾FFFD）。
|
||||
/*func TranslateToFFFE2(pSrc []byte) []byte {
|
||||
var (
|
||||
srcPos, tgtPos, pos1, pos2, iii uint16
|
||||
gap uint16
|
||||
got1stFF bool
|
||||
)
|
||||
|
||||
if pSrc == nil {
|
||||
return nil // 入口参数错误
|
||||
}
|
||||
|
||||
pTgt := make([]byte, 0, len(pSrc)*2) // 预分配空间以应对最坏情况
|
||||
|
||||
for srcPos = 0; srcPos < uint16(len(pSrc)); srcPos++ {
|
||||
if got1stFF {
|
||||
if pSrc[srcPos] == 0xFF {
|
||||
// 已遇到前一个FF，且当前又遇到FF
|
||||
|
||||
pos2 = srcPos
|
||||
pTgt = append(pTgt, byte(pos2-pos1))
|
||||
tgtPos++
|
||||
|
||||
for iii = pos1 + 1; iii < pos2; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
tgtPos++
|
||||
}
|
||||
|
||||
got1stFF = false
|
||||
pos1, pos2, gap = 0, 0, 0
|
||||
} else {
|
||||
// 已遇到前一个FF，且当前遇到非FF
|
||||
gap++
|
||||
if gap == 252 {
|
||||
got1stFF, gap = false, 0
|
||||
pTgt = append(pTgt, 0)
|
||||
tgtPos++
|
||||
|
||||
for iii = pos1 + 1; iii <= srcPos; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
tgtPos++
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
pTgt = append(pTgt, pSrc[srcPos])
|
||||
tgtPos++
|
||||
|
||||
if pSrc[srcPos] == 0xFF {
|
||||
// 遇到前一个FF
|
||||
got1stFF = true
|
||||
pos1 = srcPos
|
||||
gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 已经遇到了前一个FF, 且源数据已到了末尾仍未遇到后一个FF
|
||||
if got1stFF {
|
||||
pTgt = append(pTgt, 0)
|
||||
tgtPos++
|
||||
|
||||
for iii = pos1 + 1; iii < uint16(len(pSrc)); iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
tgtPos++
|
||||
}
|
||||
}
|
||||
|
||||
// 截取实际使用的部分返回
|
||||
return pTgt[:tgtPos]
|
||||
}
|
||||
*/
|
||||
func TranslateToFFFE3(pSrc []uint8, SrcLen uint16) ([]byte, uint16) {
|
||||
var (
|
||||
SrcPos, TgtPos, Pos1, Pos2, iii uint16
|
||||
Gap uint16
|
||||
Got1stFF uint8
|
||||
pTgt []uint8
|
||||
)
|
||||
|
||||
if pSrc == nil {
|
||||
fmt.Println("入口参数错误")
|
||||
return nil, 0
|
||||
}
|
||||
|
||||
pTgt = make([]uint8, 0, SrcLen*2) // 预分配足够的空间以避免频繁扩容
|
||||
TgtPos = 0
|
||||
|
||||
for SrcPos = 0; SrcPos < SrcLen; SrcPos++ {
|
||||
if Got1stFF == 1 {
|
||||
if pSrc[SrcPos] == 0xFF {
|
||||
// 已遇到前一个FF，且当前又遇到FF
|
||||
//Got2ndFF = 1
|
||||
Pos2 = SrcPos
|
||||
pTgt = append(pTgt, uint8(Pos2-Pos1-1))
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii < Pos2; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
|
||||
Got1stFF = 0
|
||||
//Got2ndFF = 0
|
||||
Pos1 = 0
|
||||
Pos2 = 0
|
||||
Gap = 0
|
||||
} else {
|
||||
// 已遇到前一个FF，且当前遇到非FF
|
||||
Gap++
|
||||
if Gap == 252 {
|
||||
Got1stFF = 0
|
||||
Gap = 0
|
||||
pTgt = append(pTgt, 0)
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii <= SrcPos; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
pTgt = append(pTgt, pSrc[SrcPos])
|
||||
TgtPos++
|
||||
if pSrc[SrcPos] == 0xFF {
|
||||
// 遇到前一个FF
|
||||
Got1stFF = 1
|
||||
Pos1 = SrcPos
|
||||
Gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 已经遇到了前一个FF, 且源数据已到了末尾仍未遇到后一个FF
|
||||
if Got1stFF == 1 {
|
||||
pTgt = append(pTgt, 0)
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii < SrcLen; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
|
||||
return pTgt, TgtPos
|
||||
func parseIdFrame(frame byte, data []byte) BtmVobcIdCommand {
|
||||
buf := bytes.NewBuffer(data)
|
||||
dataLen, _ := buf.ReadByte()
|
||||
dsn, _ := buf.ReadByte()
|
||||
var btmId uint16
|
||||
var vobcId uint16
|
||||
var vobcLifeId uint32
|
||||
var yuliu [5]byte
|
||||
var crc32 uint32
|
||||
binary.Read(buf, binary.BigEndian, &btmId)
|
||||
binary.Read(buf, binary.BigEndian, &vobcId)
|
||||
binary.Read(buf, binary.BigEndian, &vobcLifeId)
|
||||
binary.Read(buf, binary.BigEndian, &yuliu)
|
||||
binary.Read(buf, binary.BigEndian, &crc32)
|
||||
return BtmVobcIdCommand{BaseBtmVobc: BaseBtmVobc{Frame: frame, FrameLen: dataLen, AutoIdFrame: dsn, Crc32: crc32}}
|
||||
}
|
||||
|
||||
type BaseBtmVobc struct {
|
||||
@ -457,67 +149,6 @@ func baseEncode(source []byte) []byte {
|
||||
return data
|
||||
}
|
||||
|
||||
/*func TranslateToFFFE4(pSrc []byte, pTgt []byte) (int, error) {
|
||||
if pSrc == nil || pTgt == nil {
|
||||
return 0, fmt.Errorf("pSrc or pTgt is nil")
|
||||
}
|
||||
|
||||
srcLen := len(pSrc)
|
||||
tgtLen := 0
|
||||
pos1 := -1 // 前一个FF的位置
|
||||
gap := 0 // 两个FF之间的字节数
|
||||
|
||||
for i, b := range pSrc {
|
||||
if pos1 != -1 {
|
||||
if b == 0xFF {
|
||||
// 遇到了第二个FF
|
||||
pTgt[tgtLen] = byte(i - pos1 - 1) // 减1是因为不包括第一个FF
|
||||
tgtLen++
|
||||
|
||||
// 复制pos1和当前位置之间的数据
|
||||
copy(pTgt[tgtLen:tgtLen+gap], pSrc[pos1+1:i])
|
||||
tgtLen += gap
|
||||
|
||||
// 重置状态
|
||||
pos1 = -1
|
||||
gap = 0
|
||||
} else {
|
||||
// 计数两个FF之间的字节数
|
||||
gap++
|
||||
if gap == 252 {
|
||||
// 如果gap超过252，则写入一个0并复制数据
|
||||
pTgt[tgtLen] = 0
|
||||
tgtLen++
|
||||
copy(pTgt[tgtLen:tgtLen+gap], pSrc[pos1+1:i])
|
||||
tgtLen += gap
|
||||
// 重置状态
|
||||
pos1 = -1
|
||||
gap = 0
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
pTgt[tgtLen] = b
|
||||
tgtLen++
|
||||
if b == 0xFF {
|
||||
// 遇到了第一个FF
|
||||
pos1 = i
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果数据以FF结尾，但没有第二个FF
|
||||
if pos1 != -1 {
|
||||
pTgt[tgtLen] = 0
|
||||
tgtLen++
|
||||
// 复制pos1之后的数据
|
||||
copy(pTgt[tgtLen:tgtLen+srcLen-pos1-1], pSrc[pos1+1:])
|
||||
tgtLen += srcLen - pos1 - 1
|
||||
}
|
||||
|
||||
return tgtLen, nil
|
||||
}
|
||||
*/
|
||||
// ID 命令帧的正文
|
||||
type BtmVobcIdCommand struct {
|
||||
BaseBtmVobc
|
||||
@ -546,6 +177,7 @@ const (
|
||||
REQ_FRAME_STATUS_OK = 0x06
|
||||
REQ_FRAME_STATUS_ERROR = 0x15
|
||||
REQ_FRAME_STATUS_BOOT = 0x00
|
||||
|
||||
REQ_PACKETS_TYPE_FREE = 0x05
|
||||
REQ_PACKETS_TYPE_MSG = 0x0A
|
||||
REQ_PACKETS_TYPE_BOOT = 0x00
|
||||
@ -564,11 +196,6 @@ type BtmVobcReq struct {
|
||||
VobcLifeWalkDistance uint16 //VOBC 周期走行距离 单位：cm
|
||||
}
|
||||
|
||||
const (
|
||||
a = 0x06
|
||||
b = 0x15
|
||||
)
|
||||
|
||||
func (b *BtmVobcReq) Decode(data []byte) {
|
||||
base, buf := baseDecode(data)
|
||||
|
||||
@ -594,34 +221,8 @@ func (b *BtmVobcReq) Decode(data []byte) {
|
||||
b.VobcLifeWalkDistance = walkDis
|
||||
}
|
||||
|
||||
var MESSAGE_AUTO_ID byte = 1
|
||||
var MESSAGE_SERIAL byte = 1
|
||||
var lock sync.Mutex
|
||||
|
||||
func getAutoId() byte {
|
||||
defer lock.Unlock()
|
||||
lock.Lock()
|
||||
if MESSAGE_AUTO_ID <= 0 {
|
||||
MESSAGE_AUTO_ID = 1
|
||||
} else if MESSAGE_AUTO_ID > 255 {
|
||||
MESSAGE_AUTO_ID = 1
|
||||
}
|
||||
MESSAGE_AUTO_ID += 1
|
||||
return MESSAGE_AUTO_ID
|
||||
}
|
||||
|
||||
func GetAutoMessageId() byte {
|
||||
defer lock.Unlock()
|
||||
lock.Lock()
|
||||
if MESSAGE_SERIAL <= 0 {
|
||||
MESSAGE_SERIAL = 1
|
||||
} else if MESSAGE_SERIAL > 255 {
|
||||
MESSAGE_SERIAL = 1
|
||||
}
|
||||
MESSAGE_SERIAL += 1
|
||||
return MESSAGE_SERIAL
|
||||
}
|
||||
|
||||
const (
|
||||
btm_status_ok = 0x00
|
||||
btm_status_warn = 0x04
|
||||
@ -658,8 +259,9 @@ func (b *BtmVobcMessage) Encode() []byte {
|
||||
|
||||
binary.Write(buf, binary.BigEndian, byte(MESSAGE_TYPE))
|
||||
binary.Write(buf, binary.BigEndian, byte(0x87))
|
||||
|
||||
//binary.Write(buf, binary.BigEndian, b.AutoIdFrame)
|
||||
binary.Write(buf, binary.BigEndian, getAutoId())
|
||||
binary.Write(buf, binary.BigEndian, b.AutoIdFrame)
|
||||
binary.Write(buf, binary.BigEndian, b.FontTtl)
|
||||
binary.Write(buf, binary.BigEndian, b.MsgSerial)
|
||||
binary.Write(buf, binary.BigEndian, b.BtmStatus)
|
||||
@ -724,8 +326,8 @@ func (b *BtmVobcMsgFree) Encode() []byte {
|
||||
|
||||
binary.Write(buf, binary.BigEndian, byte(FREE_MESSAGE_TYPE))
|
||||
binary.Write(buf, binary.BigEndian, byte(0x87))
|
||||
binary.Write(buf, binary.BigEndian, getAutoId())
|
||||
//binary.Write(buf, binary.BigEndian, b.AutoIdFrame)
|
||||
|
||||
binary.Write(buf, binary.BigEndian, b.AutoIdFrame)
|
||||
binary.Write(buf, binary.BigEndian, byte(0)) //保留
|
||||
binary.Write(buf, binary.BigEndian, byte(0)) //保留
|
||||
binary.Write(buf, binary.BigEndian, b.MsgSerial)
|
||||
|
||||
283
third_party/message/btm_vobc_translate_fffe.go
vendored
Normal file
283
third_party/message/btm_vobc_translate_fffe.go
vendored
Normal file
@ -0,0 +1,283 @@
|
||||
package message
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
)
|
||||
|
||||
func aa(src []byte, dest []byte) uint16 {
|
||||
var (
|
||||
SrcPos, TgtPos, Pos1, Pos2, iii, Gap uint16
|
||||
Got1stFF uint8
|
||||
)
|
||||
srouceLen := uint16(len(src))
|
||||
for SrcPos = 0; SrcPos < srouceLen; SrcPos++ {
|
||||
if Got1stFF == 1 {
|
||||
if src[SrcPos] == 0xff {
|
||||
//Got2ndFF = 1
|
||||
Pos2 = SrcPos
|
||||
dest[TgtPos] = byte(Pos2 - Pos1)
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii < Pos2; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
Got1stFF = 0
|
||||
//Got2ndFF = 0
|
||||
Pos1 = 0
|
||||
Pos2 = 0
|
||||
Gap = 0
|
||||
} else {
|
||||
/*已遇到前一个FF，且当前遇到非FF*/
|
||||
Gap++
|
||||
if 252 == Gap {
|
||||
Got1stFF = 0
|
||||
Gap = 0
|
||||
dest[TgtPos] = 0
|
||||
TgtPos++
|
||||
for iii = Pos1 + 1; iii <= SrcPos; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
/*尚未遇到前一个FF*/
|
||||
dest[TgtPos] = src[SrcPos]
|
||||
TgtPos++
|
||||
if 0xFF == src[SrcPos] {
|
||||
/*遇到前一个FF*/
|
||||
Got1stFF = 1
|
||||
Pos1 = SrcPos
|
||||
Gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
if 1 == Got1stFF {
|
||||
dest[TgtPos] = 0
|
||||
TgtPos++
|
||||
for iii = Pos1 + 1; iii < srouceLen; iii++ {
|
||||
dest[TgtPos] = src[iii]
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
return TgtPos
|
||||
}
|
||||
|
||||
func TranslateFromFFFE(pSrc []byte) ([]byte, uint16) {
|
||||
var (
|
||||
srcPos, tgtPos, nonFFCount, nextFFPos uint16
|
||||
char uint8
|
||||
state int
|
||||
)
|
||||
|
||||
if len(pSrc) == 0 {
|
||||
return nil, 0 // 入参错误
|
||||
}
|
||||
|
||||
pTgt := make([]byte, len(pSrc)) // 预分配与输入等长的缓冲区
|
||||
tgtPos = 0
|
||||
state = WAIT_FF_C1
|
||||
nonFFCount = 0
|
||||
|
||||
for srcPos = 0; srcPos < uint16(len(pSrc)); srcPos++ {
|
||||
char = pSrc[srcPos]
|
||||
switch state {
|
||||
case WAIT_FF_C1:
|
||||
if char == 0xFF {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
state = WAIT_FF_C2
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
}
|
||||
case WAIT_FF_C2:
|
||||
if char == 0xFD || char == 0xFE || char == 0xFF {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
if char == 0 {
|
||||
state = WAIT_NO_FF
|
||||
} else if char == 1 {
|
||||
pTgt[tgtPos] = 0xFF
|
||||
tgtPos++
|
||||
state = WAIT_FF_C1
|
||||
} else {
|
||||
nextFFPos = srcPos + uint16(char) - 1
|
||||
state = WAIT_FF_POS
|
||||
}
|
||||
}
|
||||
case WAIT_NO_FF:
|
||||
nonFFCount++
|
||||
if char == 0xFF && nonFFCount < 252 {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
if nonFFCount == 252 {
|
||||
nonFFCount = 0
|
||||
state = WAIT_FF_C1
|
||||
}
|
||||
}
|
||||
case WAIT_FF_POS:
|
||||
if char == 0xFF {
|
||||
state = ABNORMAL_STATE
|
||||
} else {
|
||||
pTgt[tgtPos] = char
|
||||
tgtPos++
|
||||
if srcPos == nextFFPos {
|
||||
pTgt[tgtPos] = 0xFF
|
||||
tgtPos++
|
||||
state = WAIT_FF_C1
|
||||
}
|
||||
}
|
||||
default:
|
||||
state = ABNORMAL_STATE
|
||||
}
|
||||
|
||||
if state == ABNORMAL_STATE {
|
||||
tgtPos = 0
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
// 退出时的状态判断
|
||||
if state == WAIT_FF_C2 || state == WAIT_FF_POS {
|
||||
tgtPos = 0
|
||||
}
|
||||
|
||||
return pTgt[:tgtPos], tgtPos
|
||||
}
|
||||
|
||||
// TranslateToFFFE 对给定的字节切片进行FFFE转义处理
|
||||
func TranslateToFFFE(src []byte) ([]byte, error) {
|
||||
if src == nil {
|
||||
return nil, fmt.Errorf("source data is nil")
|
||||
}
|
||||
var tgt []byte
|
||||
var pos1, pos2 int
|
||||
var gap int
|
||||
var got1stFF bool
|
||||
|
||||
for i, b := range src {
|
||||
if got1stFF {
|
||||
if b == 0xFF {
|
||||
// 已遇到前一个FF，且当前又遇到FF
|
||||
got1stFF = false
|
||||
pos2 = i
|
||||
if gap > 252 {
|
||||
// 间隙过大，特殊处理
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:pos2]...)
|
||||
} else {
|
||||
// 写入间隙长度
|
||||
tgt = append(tgt, byte(gap))
|
||||
// 写入间隙中的字节
|
||||
tgt = append(tgt, src[pos1+1:pos2]...)
|
||||
}
|
||||
} else {
|
||||
// 已遇到前一个FF，且当前遇到非FF，增加gap计数
|
||||
gap++
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
tgt = append(tgt, b)
|
||||
if b == 0xFF {
|
||||
// 遇到前一个FF
|
||||
got1stFF = true
|
||||
pos1 = i
|
||||
gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 如果在数据末尾遇到了FF且没有第二个FF
|
||||
if got1stFF {
|
||||
if gap > 252 {
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:]...)
|
||||
} else {
|
||||
tgt = append(tgt, 0)
|
||||
tgt = append(tgt, src[pos1+1:len(src)]...)
|
||||
}
|
||||
}
|
||||
|
||||
return tgt, nil
|
||||
}
|
||||
|
||||
func TranslateToFFFE3(pSrc []uint8, SrcLen uint16) ([]byte, uint16) {
|
||||
var (
|
||||
SrcPos, TgtPos, Pos1, Pos2, iii uint16
|
||||
Gap uint16
|
||||
Got1stFF uint8
|
||||
pTgt []uint8
|
||||
)
|
||||
|
||||
if pSrc == nil {
|
||||
fmt.Println("入口参数错误")
|
||||
return nil, 0
|
||||
}
|
||||
|
||||
pTgt = make([]uint8, 0, SrcLen*2) // 预分配足够的空间以避免频繁扩容
|
||||
TgtPos = 0
|
||||
|
||||
for SrcPos = 0; SrcPos < SrcLen; SrcPos++ {
|
||||
if Got1stFF == 1 {
|
||||
if pSrc[SrcPos] == 0xFF {
|
||||
// 已遇到前一个FF，且当前又遇到FF
|
||||
//Got2ndFF = 1
|
||||
Pos2 = SrcPos
|
||||
pTgt = append(pTgt, uint8(Pos2-Pos1-1))
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii < Pos2; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
|
||||
Got1stFF = 0
|
||||
//Got2ndFF = 0
|
||||
Pos1 = 0
|
||||
Pos2 = 0
|
||||
Gap = 0
|
||||
} else {
|
||||
// 已遇到前一个FF，且当前遇到非FF
|
||||
Gap++
|
||||
if Gap == 252 {
|
||||
Got1stFF = 0
|
||||
Gap = 0
|
||||
pTgt = append(pTgt, 0)
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii <= SrcPos; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// 尚未遇到前一个FF
|
||||
pTgt = append(pTgt, pSrc[SrcPos])
|
||||
TgtPos++
|
||||
if pSrc[SrcPos] == 0xFF {
|
||||
// 遇到前一个FF
|
||||
Got1stFF = 1
|
||||
Pos1 = SrcPos
|
||||
Gap = 0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 已经遇到了前一个FF, 且源数据已到了末尾仍未遇到后一个FF
|
||||
if Got1stFF == 1 {
|
||||
pTgt = append(pTgt, 0)
|
||||
TgtPos++
|
||||
|
||||
for iii = Pos1 + 1; iii < SrcLen; iii++ {
|
||||
pTgt = append(pTgt, pSrc[iii])
|
||||
TgtPos++
|
||||
}
|
||||
}
|
||||
|
||||
return pTgt, TgtPos
|
||||
}
|
||||
270
third_party/semi_physical_train/btm_vobc.go
vendored
Normal file
270
third_party/semi_physical_train/btm_vobc.go
vendored
Normal file
@ -0,0 +1,270 @@
|
||||
package semi_physical_train
|
||||
|
||||
import (
|
||||
"context"
|
||||
"encoding/hex"
|
||||
"fmt"
|
||||
"github.com/google/uuid"
|
||||
"joylink.club/bj-rtsts-server/config"
|
||||
"joylink.club/bj-rtsts-server/const/balise_const"
|
||||
"joylink.club/bj-rtsts-server/dto/state_proto"
|
||||
"joylink.club/bj-rtsts-server/third_party/can_btm"
|
||||
"joylink.club/bj-rtsts-server/third_party/message"
|
||||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||||
"log/slog"
|
||||
"math"
|
||||
"time"
|
||||
|
||||
"strings"
|
||||
"sync"
|
||||
)
|
||||
|
||||
type BtmVobcManage interface {
|
||||
GetBtmVobcConfig() config.BtmVobcConfig
|
||||
|
||||
GetAllTrain() []*state_proto.TrainState
|
||||
GetConnVobcTrain() *state_proto.TrainState
|
||||
}
|
||||
type BtmVobcService interface {
|
||||
Start(btmVobcManage BtmVobcManage)
|
||||
Stop()
|
||||
SendData(data []byte)
|
||||
}
|
||||
type BtmVobcClient struct {
|
||||
calFun context.CancelFunc
|
||||
client udp.UdpClient
|
||||
server udp.UdpServer
|
||||
manage BtmVobcManage
|
||||
}
|
||||
|
||||
var (
|
||||
btmVobcLocker sync.Mutex
|
||||
btmVobcClient *BtmVobcClient
|
||||
btmVobcBaliseLocker sync.Mutex
|
||||
//最新接受数据时间
|
||||
reviceTimeStamp int64
|
||||
)
|
||||
|
||||
func BtmDefault() BtmVobcService {
|
||||
defer btmVobcLocker.Unlock()
|
||||
btmVobcLocker.Lock()
|
||||
if btmVobcClient == nil {
|
||||
btmVobcClient = &BtmVobcClient{}
|
||||
}
|
||||
return btmVobcClient
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) Start(btmVobcManage BtmVobcManage) {
|
||||
cfg := btmVobcManage.GetBtmVobcConfig()
|
||||
if !cfg.Open {
|
||||
slog.Info("11号线 btm vobc配置未开启...")
|
||||
return
|
||||
}
|
||||
udpServer := udp.NewServer(fmt.Sprintf("%v:%d", cfg.LocalUdpIp, cfg.LocalUdpPort), b.handleBtmVobcFrames)
|
||||
err := udpServer.Listen()
|
||||
if err != nil {
|
||||
slog.Error("11号线 btm VOBC 服务启动失败...")
|
||||
return
|
||||
}
|
||||
//
|
||||
udpClient := udp.NewClient(fmt.Sprintf("%s:%d", cfg.RemoteIp, cfg.RemoteUdpPort))
|
||||
b.manage = btmVobcManage
|
||||
b.server = udpServer
|
||||
b.client = udpClient
|
||||
reviceTimeStamp = time.Now().UnixMilli()
|
||||
_, calFun := context.WithCancel(context.Background())
|
||||
b.calFun = calFun
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) vobcBtmQueryHandle(data []byte) {
|
||||
train := b.manage.GetConnVobcTrain()
|
||||
if train == nil {
|
||||
slog.Error("vobc btm 未找到VOBC连接的列车...")
|
||||
return
|
||||
}
|
||||
requestId := uuid.New().String()
|
||||
frameType, dataText, err := message.BtmVobcDecode(data)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("%v,请求id:%v", err, requestId))
|
||||
return
|
||||
}
|
||||
receiveDataTime := time.Now().UnixMilli()
|
||||
if frameType == message.COMMAND_TYPE {
|
||||
idCommand := &message.BtmVobcIdCommand{}
|
||||
idCommand.Decode(dataText)
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的id命令帧,requestId:%v,接受时间:%v", requestId, receiveDataTime))
|
||||
if train.VobcState.VobcBtmInfo == nil {
|
||||
train.VobcState.VobcBtmInfo = &state_proto.TrainVobcState_VobcBtmInfo{BtmId: uint32(idCommand.BtmId), VobcId: uint32(idCommand.VobcId), VobcLifeId: idCommand.VobcLifeNum}
|
||||
}
|
||||
} else if train.VobcState.VobcBtmInfo != nil && frameType == message.REQUEST_TYPE {
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的请求帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
|
||||
req := &message.BtmVobcReq{}
|
||||
req.Decode(dataText)
|
||||
b.requestFramePackets(requestId, req, train, receiveDataTime)
|
||||
|
||||
} else {
|
||||
//slog.Error(fmt.Sprintf("btm vobc 解析未知命令帧类型:0x%v,原始数据:%v,长度:%v,requestId:%v", strconv.FormatInt(int64(frameType), 16), hex.EncodeToString(cfs), len(cfs), requestId))
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) handleBtmVobcFrames(cfs []byte) {
|
||||
/* reviceTimeStamp = time.Now().UnixMilli()
|
||||
train := b.manage.GetConnVobcTrain()
|
||||
if train == nil {
|
||||
//slog.Error("vobc btm 未找到VOBC连接的列车...")
|
||||
return
|
||||
}
|
||||
requestId := uuid.New().String()
|
||||
slog.Info(fmt.Sprintf("获取到vobc btm原始数据:%v, requestId:%v", hex.EncodeToString(cfs), requestId))
|
||||
frameType, dataText, err := message.BtmVobcDecode(cfs)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("%v,请求id:%v", err, requestId))
|
||||
return
|
||||
}
|
||||
receiveDataTime := time.Now().UnixMicro()
|
||||
decodePayMicoTime := (receiveDataTime - receiveDataTime) / 100
|
||||
if frameType == message.COMMAND_TYPE {
|
||||
idCommand := &message.BtmVobcIdCommand{}
|
||||
idCommand.Decode(dataText)
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的id命令帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
|
||||
b.packets(requestId, idCommand.VobcLifeNum, idCommand.AutoIdFrame, receiveDataTime, decodePayMicoTime, train.VobcBtm)
|
||||
} else if frameType == message.REQUEST_TYPE {
|
||||
slog.Info(fmt.Sprintf("成功接受btm vobc的请求帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
|
||||
req := &message.BtmVobcReq{}
|
||||
req.Decode(dataText)
|
||||
b.RequestFramePackets(req.VobcLifeNum, req.AutoIdFrame, requestId, receiveDataTime, decodePayMicoTime, req, train.VobcBtm)
|
||||
} else {
|
||||
slog.Error(fmt.Sprintf("btm vobc 解析未知命令帧类型:0x%v,原始数据:%v,长度:%v,requestId:%v", strconv.FormatInt(int64(frameType), 16), hex.EncodeToString(cfs), len(cfs), requestId))
|
||||
return
|
||||
}*/
|
||||
}
|
||||
func createFreeBalisePacketString() string {
|
||||
return strings.Repeat("00", balise_const.UserTelegramByteLen)
|
||||
}
|
||||
func createFreeBalisePackets() []byte {
|
||||
str := createFreeBalisePacketString()
|
||||
d, _ := hex.DecodeString(str)
|
||||
return d
|
||||
}
|
||||
|
||||
// 请求帧
|
||||
func (b *BtmVobcClient) requestFramePackets(requestId string, req *message.BtmVobcReq, train *state_proto.TrainState, receiveTime int64) {
|
||||
|
||||
if req.FrameStatus == message.REQ_FRAME_STATUS_BOOT && req.MessageType == message.REQ_PACKETS_TYPE_BOOT {
|
||||
//vobc 上电，清空应答器
|
||||
can_btm.ClearBalise(train)
|
||||
} else {
|
||||
var (
|
||||
btmState *state_proto.BTMState
|
||||
dsn byte
|
||||
matcher bool
|
||||
)
|
||||
ms := req.MessageSerial
|
||||
|
||||
if req.FrameStatus == message.REQ_FRAME_STATUS_ERROR {
|
||||
//抢答器错误，重新发送
|
||||
btmState, dsn, _, _ = can_btm.FindBaliseResend(train)
|
||||
} else {
|
||||
//判断 报文序列号与之前发送的 是否一致,不一致
|
||||
//如果一致返回新的应答器，如果不一致返回之前发送的应答器,如果不一致并且没有找到之前发送的应答器，则返回新应答器
|
||||
btmState, dsn, matcher = can_btm.FindBaliseByMessageSerial(train, req.MessageSerial)
|
||||
if matcher {
|
||||
if btmState != nil {
|
||||
//正常应答
|
||||
btmState.IsSend = true
|
||||
ms = req.MessageSerial + 1
|
||||
if ms > 255 {
|
||||
ms = 1
|
||||
}
|
||||
}
|
||||
} else {
|
||||
//vobc 未收到应答器，重新发送
|
||||
}
|
||||
}
|
||||
b.packets(requestId, btmState, ms, dsn, req.VobcLifeNum, receiveTime)
|
||||
}
|
||||
}
|
||||
|
||||
// 有应答器报文
|
||||
func (b *BtmVobcClient) balisePackets(telegram, requestId string, messageSerial, dsn byte, vobcLifeNum uint32, receiveTime int64) {
|
||||
|
||||
data, e := hex.DecodeString(telegram)
|
||||
if e != nil {
|
||||
slog.Error(fmt.Sprintf("解析应答器报文失败应答器报文长度:%v", telegram), e)
|
||||
return
|
||||
}
|
||||
//前沿时间
|
||||
var fttl uint16 = 0
|
||||
tmpFttl := int64(math.Abs(float64(time.Now().UnixMilli() - receiveTime)))
|
||||
if tmpFttl >= 0xffff {
|
||||
fttl = 0xffff
|
||||
} else {
|
||||
fttl = uint16(tmpFttl)
|
||||
}
|
||||
|
||||
var bttl uint16 = 0
|
||||
tmpBttl := int64(math.Abs(float64(time.Now().UnixMilli() - receiveTime)))
|
||||
if tmpBttl >= 0xffff {
|
||||
bttl = 0xffff
|
||||
} else {
|
||||
bttl = uint16(tmpBttl)
|
||||
}
|
||||
repTimeMicro := (time.Now().UnixMilli() - receiveTime) / 10
|
||||
baliseMsg := &message.BtmVobcMessage{FontTtl: fttl, BtmStatus: message.BTM_STSTUS_NORMAL, DecodeTime: uint16(repTimeMicro),
|
||||
BackTtl: bttl, ResponseTime: byte(repTimeMicro), MsgSerial: messageSerial, BtmMsg: data,
|
||||
VobcLifeNum: vobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: dsn}}
|
||||
baliseData := baliseMsg.Encode()
|
||||
slog.Info(fmt.Sprintf("发送btm vobc 报文数据 报文序列id:%v 报文内容:%X 长度:%v ,requestId:%v", messageSerial, baliseData, len(baliseData), requestId))
|
||||
err := b.client.Send(baliseData)
|
||||
if err != nil {
|
||||
slog.Error(fmt.Sprintf("发送btm vobc 报文数据 报文序列id:%v 报文内容:%X 长度:%v ,requestId:%v", messageSerial, baliseData, len(baliseData), requestId), err)
|
||||
return
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
// 无应答器报文
|
||||
func (b *BtmVobcClient) balisePacketsFree(requestId string, receiveTime int64, vobcLifeNum uint32, autoCommandId, messageSerial byte) {
|
||||
repTimeMicro := (time.Now().UnixMicro() - receiveTime) / 10
|
||||
freeMsg := &message.BtmVobcMsgFree{BtmStatus: message.BTM_STSTUS_NORMAL, Fun1: 0xffff, Fun2: 0x00CF, Fun3: uint16(0), Fun4: uint16(0),
|
||||
FreeMsg: createFreeBalisePackets(), RespTime: byte(repTimeMicro), MsgSerial: messageSerial, VobcLifeNum: vobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: autoCommandId}}
|
||||
freeData := freeMsg.Encode()
|
||||
logStr := fmt.Sprintf("发送btm vobc 报文序列id:%v 空闲帧报文:%X 长度:%v ,requestId:%v", messageSerial, freeData, len(freeData), requestId)
|
||||
slog.Info(logStr)
|
||||
err := b.client.Send(freeData)
|
||||
if err != nil {
|
||||
slog.Error(logStr, err)
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
// 应答器报文或空报文
|
||||
func (b *BtmVobcClient) packets(requestId string, btmState *state_proto.BTMState, messageSerial, dsn byte, vobcLifeNum uint32, receiveTime int64) {
|
||||
if btmState == nil {
|
||||
b.balisePacketsFree(requestId, receiveTime, vobcLifeNum, dsn, messageSerial)
|
||||
} else {
|
||||
b.balisePackets(btmState.Telegram, requestId, messageSerial, dsn, vobcLifeNum, receiveTime)
|
||||
}
|
||||
}
|
||||
|
||||
func (b *BtmVobcClient) SendData(data []byte) {
|
||||
if b.client != nil {
|
||||
slog.Info(fmt.Sprintf("发送btm vobc 报文:%v 长度:%v", hex.EncodeToString(data), len(data)))
|
||||
err := b.client.Send(data)
|
||||
if err != nil {
|
||||
slog.Error("发送btm vobc 报文失败:", err)
|
||||
return
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
func (b *BtmVobcClient) Stop() {
|
||||
if b.server != nil {
|
||||
b.calFun()
|
||||
b.server.Close()
|
||||
}
|
||||
if b.client != nil {
|
||||
b.client.Close()
|
||||
}
|
||||
}
|
||||
@ -1,4 +1,4 @@
|
||||
package btm_vobc
|
||||
package semi_physical_train
|
||||
|
||||
import (
|
||||
"encoding/hex"
|
||||
@ -163,7 +163,7 @@ func TestDecode232(t *testing.T) {
|
||||
|
||||
func TestDocode(t *testing.T) {
|
||||
fmt.Println(time.Microsecond * 200)
|
||||
ss := "fffe9287d300056f006464000a000000000000000000000490007f8181b60b10183280003fff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff000a00221eedf0be28e9fffd"
|
||||
ss := "fffee601804f004d0032d8e1002a006aff0020fc60316e299122ea0000000000000000000000000000e1d83000000000000000000000768888cc4961fffd"
|
||||
cfs, _ := hex.DecodeString(ss)
|
||||
frameType, dataText, err := message.BtmVobcDecode(cfs)
|
||||
fmt.Println(frameType)
|
||||
@ -1,4 +1,4 @@
|
||||
package btm_vobc
|
||||
package semi_physical_train
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
@ -96,6 +96,7 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
|
||||
status.TailOffset = tailDeviceOffset
|
||||
status.TailDevicePort = tailDevicePort
|
||||
status.BtmBaliseCache = &state_proto.TrainBtmCache{BaliseList: make([]*state_proto.BTMState, 3)}
|
||||
|
||||
//初始化列车参数状态
|
||||
createOrUpdateStateDynamicConfig(status, configTrainData, trainEndsA, trainEndsB)
|
||||
tl := status.TrainLoad * 100
|
||||
@ -107,7 +108,7 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
|
||||
status.VobcState = vobc
|
||||
|
||||
status.Tcc = initTrainTcc(vs)
|
||||
status.VobcBtm = &state_proto.VobcBtmState{TelegramState: make([]*state_proto.VobcBtmState_TelegramState, 3), History: make(map[uint32]*state_proto.VobcBtmState_VobcBtmHistoryState)}
|
||||
//status.VobcBtm = &state_proto.VobcBtmState{TelegramState: make([]*state_proto.VobcBtmState_TelegramState, 3), History: make(map[uint32]*state_proto.VobcBtmState_VobcBtmHistoryState)}
|
||||
linkIdInt, _ := strconv.Atoi(linkId)
|
||||
err := dynamics.Default().RequestAddTrain(&message.InitTrainInfo{
|
||||
TrainIndex: uint16(trainIndex),
|
||||
@ -410,6 +411,7 @@ func updateTrainBtmPosition(vs *VerifySimulation, info *message.DynamicsTrainInf
|
||||
Acceleration: info.Acceleration,
|
||||
OldLinkOffset: sta.OldLinkOffset,
|
||||
OldLink: sta.OldLink,
|
||||
IsLine12: can_btm.IsLine12(sta),
|
||||
})
|
||||
/*can_btm.Default().HandleTrainHeadPositionInfo(vs.World, sta.VobcBtm, &fi.TrainHeadPositionInfo{
|
||||
TrainId: sta.Id,
|
||||
@ -494,7 +496,8 @@ func RemoveTrainState(vs *VerifySimulation, id string) {
|
||||
if ok {
|
||||
t := d.(*state_proto.TrainState)
|
||||
err := removeTrain(vs, id, t)
|
||||
clearTrainVobcBtmState(vs, id)
|
||||
t.VobcState.VobcBtmInfo = nil
|
||||
//clearTrainVobcBtmState(vs, id)
|
||||
if err != nil {
|
||||
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: err.Error()})
|
||||
}
|
||||
@ -514,12 +517,12 @@ func calcTrailTailOffset(headerOffset, length int64, up bool) (calctailOffset in
|
||||
}
|
||||
|
||||
func clearTrainVobcBtmState(vs *VerifySimulation, id string) {
|
||||
allTrainMap := &vs.Memory.Status.TrainStateMap
|
||||
d, ok := allTrainMap.Load(id)
|
||||
if !ok {
|
||||
slog.Error(fmt.Sprintf("vobc btm 清空操作 列车【%s】不存在", id))
|
||||
return
|
||||
}
|
||||
t := d.(*state_proto.TrainState)
|
||||
t.VobcBtm.History = nil
|
||||
//allTrainMap := &vs.Memory.Status.TrainStateMap
|
||||
//d, ok := allTrainMap.Load(id)
|
||||
//if !ok {
|
||||
// slog.Error(fmt.Sprintf("vobc btm 清空操作 列车【%s】不存在", id))
|
||||
// return
|
||||
//}
|
||||
//t := d.(*state_proto.TrainState)
|
||||
//t.VobcBtm.History = nil
|
||||
}
|
||||
|
||||
@ -47,6 +47,8 @@ type VerifySimulation struct {
|
||||
UidMap map[string]*DeviceRelationship
|
||||
// 运行环境配置
|
||||
runConfig *config.ThirdPartyConfig
|
||||
//运行线路code
|
||||
ProjectCode string
|
||||
}
|
||||
|
||||
// 轨旁仿真内存模型
|
||||
|
||||
@ -857,11 +857,10 @@ func (s *VerifySimulation) TrainBtmQuery2(train *state_proto.TrainState, data []
|
||||
var balise *state_proto.BTMState
|
||||
var dsn, bc, mc byte
|
||||
if atpReq.IsResend() {
|
||||
balise, dsn, bc, mc = can_btm.Default().FindBaliseResend(train)
|
||||
balise, dsn, bc, mc = can_btm.FindBaliseResend(train)
|
||||
} else {
|
||||
balise, dsn, bc, mc = can_btm.Default().FindBaliseByNotSend(train)
|
||||
balise, dsn, bc, mc = can_btm.FindBaliseByNotSend(train)
|
||||
}
|
||||
balise, dsn, bc, mc = can_btm.Default().FindBaliseByNotSend(train)
|
||||
cl := clock(atpReq)
|
||||
btmRepFrame := createBtmStatus(trainAtm.CanId.ID4, balise, atpReq, cl, dsn, bc, mc)
|
||||
timeSyncF := message.NewBtmTimeSyncCheckFrame(trainAtm.CanId.ID4)
|
||||
@ -917,8 +916,6 @@ func createBtmStatus(canIdSn byte, btmState *state_proto.BTMState, atpReq *messa
|
||||
if btmState != nil && btmState.AboveBalise {
|
||||
statusF.DetailedCode = 0x07
|
||||
}
|
||||
|
||||
//btmState.DataSerialNumber = uint32(dsn)
|
||||
statusF.Dsn = dsn
|
||||
return statusF
|
||||
}
|
||||
@ -1016,6 +1013,6 @@ func (s *VerifySimulation) ObtainTrainDigitalMockData(train *state_proto.TrainSt
|
||||
msgArr = append(msgArr, message.TrainPcSimBaseMessage{Data: []byte{message.DOOR_MODE_AA, modeAA}, Type: message.SENDER_TRAIN_OUTR_INFO})
|
||||
msgArr = append(msgArr, message.TrainPcSimBaseMessage{Data: []byte{message.DOOR_MODE_AM, modeAM}, Type: message.SENDER_TRAIN_OUTR_INFO})
|
||||
msgArr = append(msgArr, message.TrainPcSimBaseMessage{Data: []byte{message.DOOR_MODE_MM, modeMM}, Type: message.SENDER_TRAIN_OUTR_INFO})
|
||||
train.BtmState = nil
|
||||
|
||||
return msgArr
|
||||
}
|
||||
|
||||
@ -2,9 +2,9 @@ package ts
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"joylink.club/bj-rtsts-server/service"
|
||||
"joylink.club/bj-rtsts-server/third_party/acc"
|
||||
axleBeijing12 "joylink.club/bj-rtsts-server/third_party/axle_device/beijing12"
|
||||
"joylink.club/bj-rtsts-server/third_party/btm_vobc"
|
||||
"joylink.club/bj-rtsts-server/third_party/interlock/beijing11"
|
||||
"joylink.club/bj-rtsts-server/third_party/interlock/beijing12"
|
||||
"joylink.club/bj-rtsts-server/third_party/radar"
|
||||
@ -51,6 +51,7 @@ func CreateSimulation(projectId int32, mapIds []int32, runConfig *dto.ProjectRun
|
||||
return "", sys_error.New("一套环境同时只能运行一个仿真")
|
||||
}
|
||||
if !e {
|
||||
project := service.QueryProject(projectId)
|
||||
verifySimulation, err := memory.CreateSimulation(projectId, mapIds, runConfig)
|
||||
|
||||
if err != nil {
|
||||
@ -68,6 +69,7 @@ func CreateSimulation(projectId int32, mapIds []int32, runConfig *dto.ProjectRun
|
||||
verifySimulation.World.Close()
|
||||
return "", err
|
||||
}
|
||||
verifySimulation.ProjectCode = project.Code
|
||||
simulationMap.Store(simulationId, verifySimulation)
|
||||
// verifySimulation.Start()
|
||||
// 全部成功，启动仿真
|
||||
@ -148,7 +150,7 @@ func runThirdParty(s *memory.VerifySimulation) error {
|
||||
acc.Default().Start(s)
|
||||
train_pc_sim.Default().Start(s)
|
||||
//btm vobc
|
||||
btm_vobc.Default().Start(s)
|
||||
semi_physical_train.BtmDefault().Start(s)
|
||||
return nil
|
||||
}
|
||||
|
||||
@ -167,7 +169,7 @@ func stopThirdParty(s *memory.VerifySimulation) {
|
||||
//列车PC仿真停止
|
||||
train_pc_sim.Default().Stop()
|
||||
|
||||
btm_vobc.Default().Stop()
|
||||
semi_physical_train.BtmDefault().Stop()
|
||||
// 停止动力学接口功能
|
||||
dynamics.Default().Stop()
|
||||
// 联锁启动
|
||||
|
||||
Loading…
Reference in New Issue
Block a user