joylink/rts-sim-testing-service
6
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

btm vobc 模块功能

Browse Source
This commit is contained in:
tiger_zhou 2024-06-06 17:57:30 +08:00
parent 497bbad8ed
commit a58a4350c2
14 changed files with 3777 additions and 1937 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
bin/acc_conn-example.go
View File

@ -64,13 +64,59 @@ func main() {
//initAccTest()
//initSpeedTest()
select {}
for {
}
}
var testUserBtmMsg = "90007F8181B60B10183280003FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF"
var freeBtmMsg = strings.Repeat("00", balise_const.UserTelegramByteLen)
func sendPacket(lifeNum uint32, autoId byte) {
userMsg, _ := hex.DecodeString(testUserBtmMsg)
msg := &message.BtmVobcMessage{FontTtl: 5, BtmStatus: 0x00, DecodeTime: 10, BackTtl: 4, BtmMsg: userMsg, ResponseTime: 10,
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)
btmCli.Send(sendData)
/*freeMsg, _ := hex.DecodeString(freeBtmMsg)
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: message.GetAutoMessageId()}
sendData2 := msg2.Encode()
fmt.Println("发送btm vobc 空报文11111:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", autoId, "报文序列号:", msg2.MsgSerial)
btmCli.Send(sendData2)*/
}
func sendPacketFree(lifeNum uint32, autoId byte, msgs byte) {
freeMsg, _ := hex.DecodeString(freeBtmMsg)
newMsg := msgs + 1
if newMsg > 255 {
newMsg = 1
}
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}
sendData2 := msg2.Encode()
fmt.Println("发送btm vobc 空报文:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", autoId, "报文序列号:", msg2.MsgSerial)
btmCli.Send(sendData2)
}
func RequestFramePackets(req *message.BtmVobcReq) {
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 {
sendPacketFree(req.VobcLifeNum, req.AutoIdFrame, req.MessageSerial)
} else if req.FrameStatus == message.REQ_FRAME_STATUS_OK {
//帧正确，删除之前发送的数据
fmt.Println("11111111111111111111")
} else if req.FrameStatus == message.REQ_FRAME_STATUS_ERROR {
//帧不正确 重新发送2次，如果2次后仍然不正确，则删除之前发送的数据
fmt.Println("22222222222222222")
}
}
func handleBtmVobcFrames(cfs []byte) {
fmt.Println("收到源数据:%v", hex.EncodeToString(cfs))
frameType, dataText, err := message.BtmVobcDecode(cfs)
if err != nil {
return
@ -78,27 +124,13 @@ func handleBtmVobcFrames(cfs []byte) {
if frameType == message.COMMAND_TYPE {
idCommand := &message.BtmVobcIdCommand{}
idCommand.Decode(dataText)
//userMsg, _ := hex.DecodeString(testUserBtmMsg)
//msg := &message.BtmVobcMessage{FontTtl: 5, BtmStatus: 0x00, DecodeTime: 10, BackTtl: 4, BtmMsg: userMsg, ResponseTime: 10,
// VobcLifeNum: idCommand.VobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: idCommand.AutoIdFrame}}
//sendData := msg.Encode()
//fmt.Println("发送btm vobc len:", len(sendData), "报文:", hex.EncodeToString(sendData), "报文序列号:", msg.MsgSerial)
//btmCli.Send(sendData)
freeMsg, _ := hex.DecodeString(freeBtmMsg)
msg2 := &message.BtmVobcMsgFree{BtmStatus: 0x00, WorkTemperature: 10, Fun1: uint16(0), Fun2: uint16(0), Fun3: uint16(0), Fun4: uint16(0),
FreeMsg: freeMsg, RespTime: 20, VobcLifeNum: idCommand.VobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: idCommand.AutoIdFrame}}
sendData2 := msg2.Encode()
fmt.Println("发送btm vobc 空报文:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", idCommand.AutoIdFrame)
btmCli.Send(sendData2)
sendPacket(idCommand.VobcLifeNum, idCommand.AutoIdFrame)
} else if frameType == message.REQUEST_TYPE {
req := &message.BtmVobcReq{}
req.Decode(dataText)
fmt.Println(req, "========================")
RequestFramePackets(req)
} else {
slog.Error("btm vobc 解析未知命令帧类型", strconv.FormatInt(int64(frameType), 16), frameType, "原始数据:", hex.EncodeToString(cfs), "长度:", len(cfs))
slog.Error(fmt.Sprintf("btm vobc 解析未知命令帧类型:0x%v,原始数据:%v,长度:%v", strconv.FormatInt(int64(frameType), 16), hex.EncodeToString(cfs), len(cfs)))
return
}

2146
dto/data_proto/stationLayoutGraphics.pb.go
View File

File diff suppressed because it is too large Load Diff

2325
dto/state_proto/device_state.pb.go
View File

File diff suppressed because it is too large Load Diff

2
go.mod
View File

@ -8,7 +8,7 @@ require (
github.com/eclipse/paho.golang v0.21.0
github.com/gin-contrib/cors v1.4.0
github.com/golang/protobuf v1.5.3
github.com/google/uuid v1.4.0
github.com/google/uuid v1.6.0
github.com/sagikazarmark/slog-shim v0.1.0
github.com/snksoft/crc v1.1.0
github.com/spf13/viper v1.18.1

4
go.sum
View File

@ -83,8 +83,8 @@ github.com/google/go-cmp v0.5.5/go.mod h1:v8dTdLbMG2kIc/vJvl+f65V22dbkXbowE6jgT/
github.com/google/go-cmp v0.6.0 h1:ofyhxvXcZhMsU5ulbFiLKl/XBFqE1GSq7atu8tAmTRI=
github.com/google/go-cmp v0.6.0/go.mod h1:17dUlkBOakJ0+DkrSSNjCkIjxS6bF9zb3elmeNGIjoY=
github.com/google/gofuzz v1.0.0/go.mod h1:dBl0BpW6vV/+mYPU4Po3pmUjxk6FQPldtuIdl/M65Eg=
github.com/google/uuid v1.4.0 h1:MtMxsa51/r9yyhkyLsVeVt0B+BGQZzpQiTQ4eHZ8bc4=
github.com/google/uuid v1.4.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo=
github.com/google/uuid v1.6.0 h1:NIvaJDMOsjHA8n1jAhLSgzrAzy1Hgr+hNrb57e+94F0=
github.com/google/uuid v1.6.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo=
github.com/gorilla/websocket v1.5.1 h1:gmztn0JnHVt9JZquRuzLw3g4wouNVzKL15iLr/zn/QY=
github.com/gorilla/websocket v1.5.1/go.mod h1:x3kM2JMyaluk02fnUJpQuwD2dCS5NDG2ZHL0uE0tcaY=
github.com/hashicorp/hcl v1.0.0 h1:0Anlzjpi4vEasTeNFn2mLJgTSwt0+6sfsiTG8qcWGx4=

2
rts-sim-testing-message

@ -1 +1 @@
Subproject commit fed626ac4b0d1b2696026438d32fe812ebd00ac2
Subproject commit c1499de7bb6e41b56f53c982d516da30988fbd44

320
third_party/btm_vobc/btm_vobc.go vendored
View File

@ -3,11 +3,23 @@ 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"
"joylink.club/bj-rtsts-server/ts/simulation/wayside/memory"
"log/slog"
"math"
"reflect"
"strconv"
"time"
"strings"
"sync"
)
@ -18,6 +30,9 @@ 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
@ -27,13 +42,15 @@ type BtmVobcClient struct {
}
var (
btmVobcLocker sync.Mutex
btmVobcClient *BtmVobcClient
btmVobcLocker sync.Mutex
btmVobcClient *BtmVobcClient
btmVobcBaliseLocker sync.Mutex
//最新接受数据时间
reviceTimeStamp int64
)
func Default() BtmVobcService {
defer btmVobcLocker.Unlock()
btmVobcLocker.Lock()
if btmVobcClient == nil {
btmVobcClient = &BtmVobcClient{}
@ -41,6 +58,55 @@ func Default() BtmVobcService {
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 {
@ -58,44 +124,243 @@ func (b *BtmVobcClient) Start(btmVobcManage BtmVobcManage) {
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) handleBtmVobcFrames(cfs []byte) {
frameType, dataText, err := message.BtmVobcDecode(cfs)
if err != nil {
return
func (b *BtmVobcClient) checkTrainTTLIsOver(ctx context.Context) {
defer btmVobcBaliseLocker.Unlock()
btmVobcBaliseLocker.Lock()
for {
select {
case <-ctx.Done():
return
default:
}
slog.Info("检测网络中断情况 前沿ttl 超时...")
if time.Now().UnixMilli()-reviceTimeStamp < 1000 {
return
}
vs, ok := b.manage.(*memory.VerifySimulation)
if !ok {
slog.Error("vobc btm 模块管理端初始化错误...")
return
}
newTel := make([]*state_proto.VobcBtmState_TelegramState, 0)
vs.Memory.Status.TrainStateMap.Range(func(k, v any) bool {
train := v.(*state_proto.TrainState)
for _, state := range train.VobcBtm.TelegramState {
if time.Now().UnixMilli()-state.ArriveTime < 1000 {
newTel = append(newTel, state)
}
}
train.VobcBtm.TelegramState = newTel
return true
})
time.Sleep(time.Second * 1)
}
}
func (b *BtmVobcClient) handleBtmVobcFrames(cfs []byte) {
reviceTimeStamp = time.Now().UnixMilli()
vs, ok := b.manage.(*memory.VerifySimulation)
if !ok {
slog.Error("vobc btm 模块管理端初始化错误...")
return
}
train := vs.FindConnTrain(state_proto.TrainConnState_VOBC)
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("成功接受btm vobc的id命令帧,原数据", hex.EncodeToString(cfs))
//userMsg, _ := hex.DecodeString(testUserBtmMsg)
slog.Info(fmt.Sprintf("成功接受btm vobc的id命令帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
b.packets(requestId, idCommand, receiveDataTime, decodePayMicoTime, train.VobcBtm)
//msg := &message.BtmVobcMessage{FontTtl: 5, BtmStatus: 0x00, DecodeTime: 10, BackTtl: 4, BtmMsg: userMsg, ResponseTime: 10,
// VobcLifeNum: idCommand.VobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: idCommand.AutoIdFrame}}
//sendData := msg.Encode()
//fmt.Println("发送btm vobc len:", len(sendData), "报文:", hex.EncodeToString(sendData), "报文序列号:", msg.MsgSerial)
//btmCli.Send(sendData)
//freeMsg, _ := hex.DecodeString(freeBtmMsg)
//msg2 := &message.BtmVobcMsgFree{BtmStatus: 0x00, WorkTemperature: 10, Fun1: uint16(0), Fun2: uint16(0), Fun3: uint16(0), Fun4: uint16(0),
// FreeMsg: freeMsg, RespTime: 20, VobcLifeNum: idCommand.VobcLifeNum, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: idCommand.AutoIdFrame}}
//sendData2 := msg2.Encode()
//fmt.Println("发送btm vobc 空报文:", hex.EncodeToString(sendData2), "len:", len(sendData2), "报文序列号:", msg2.MsgSerial, "atoId=", idCommand.AutoIdFrame)
//btmCli.Send(sendData2)
} else if frameType == message.REQUEST_TYPE {
slog.Info(fmt.Sprintf("成功接受btm vobc的请求帧,requestId:%v,接受时间(微秒):%v", requestId, receiveDataTime))
req := &message.BtmVobcReq{}
req.Decode(dataText)
fmt.Println(req, "========================")
} /*else {
slog.Error("btm vobc 解析未知命令帧类型", strconv.FormatInt(int64(frameType), 16), frameType, "原始数据:", hex.EncodeToString(cfs), "长度:", len(cfs))
b.RequestFramePackets(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(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, nil, 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, WorkTemperature: 10, Fun1: uint16(0), Fun2: uint16(0), 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, idCommand *message.BtmVobcIdCommand, receiveTime int64, decodePayMicoTime int64, vobcbtm *state_proto.VobcBtmState) {
if len(vobcbtm.TelegramState) == 0 {
b.balisePacketsFree(requestId, receiveTime, idCommand.VobcLifeNum, idCommand.AutoIdFrame, vobcbtm)
} else {
b.balisePackets(requestId, vobcbtm.TelegramState[0], receiveTime, decodePayMicoTime, idCommand.VobcLifeNum, idCommand.AutoIdFrame, vobcbtm)
}
}
func (b *BtmVobcClient) SendData(data []byte) {
if b.client != nil {
slog.Info("发送btm vobc 报文:", hex.EncodeToString(data), "长度:", len(data))
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)
@ -106,6 +371,7 @@ func (b *BtmVobcClient) SendData(data []byte) {
}
func (b *BtmVobcClient) Stop() {
if b.server != nil {
b.calFun()
b.server.Close()
}
if b.client != nil {

145
third_party/btm_vobc/btm_vobc2_test.go vendored Normal file
View File

@ -0,0 +1,145 @@
package btm_vobc
import (
"encoding/hex"
"fmt"
"joylink.club/bj-rtsts-server/third_party/message"
"testing"
)
const (
WAIT_FF_C1 = 0x00
WAIT_FF_C2 = 0x01
WAIT_NO_FF = 0x02
WAIT_FF_POS = 0x03
ABNORMAL_STATE = 0x04
/*FFFE数据接收状态*/
COM_WAIT_START_FF = 0x00
COM_WAIT_START_FE = 0x01
COM_WAIT_STOP_FF = 0x02
COM_WAIT_STOP_FD = 0x03
)
// TranslateFromFFFE 将经过FFFE转义处理后的数据（已经去掉了头FFFE及尾FFFD）还原为原始数据。
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
}
func TestTss(t *testing.T) {
ss := "fffee601804f004d00921d21002a00ea014fef63995bff009122ce0000000000000000000000000000211d90000000000000000000007b07310379cefffd"
ss = "fffee601804f004d00444a1c002a00473bbfa11d4b3b029122a800000000000000000000000000001c4a42000000000000000000008970492fff00d0fffd"
dd, _ := hex.DecodeString(ss)
frameType, dataText, _ := message.BtmVobcDecode(dd)
if frameType == message.COMMAND_TYPE {
fmt.Println("1111111111111111")
} else if frameType == message.REQUEST_TYPE {
req := &message.BtmVobcReq{}
req.Decode(dataText)
fmt.Println("22222222222222222222")
} else {
fmt.Println("zzzzzzzzzzzzzzzzzzzzz")
}
}
func TestFffe(t *testing.T) {
var testUserBtmMsg = "90007F8181B60B10183280003FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF"
userMsg, _ := hex.DecodeString(testUserBtmMsg)
msg := &message.BtmVobcMessage{FontTtl: 5, BtmStatus: 0x00, DecodeTime: 10, BackTtl: 4, BtmMsg: userMsg, ResponseTime: 10,
VobcLifeNum: 123, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: 123}}
sendData := msg.Encode()
fmt.Println(hex.EncodeToString(sendData))
}
func TestDecode232(t *testing.T) {
var dd uint32 = 888888888
fmt.Println(dd)
fmt.Println(uint16(dd))
ss := "9287a8000511006464000a000000000000000000000490007f8181b60b10183280003fff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff01ff000a0023438bf4c27929"
userMsg, _ := hex.DecodeString(ss)
arr, _ := TranslateFromFFFE(userMsg)
fmt.Println(arr)
fmt.Println(hex.EncodeToString(arr))
}

57
third_party/btm_vobc/btm_vobc_test.go vendored
View File

@ -3,7 +3,9 @@ package btm_vobc
import (
"bytes"
"encoding/hex"
"encoding/json"
"fmt"
"joylink.club/bj-rtsts-server/third_party/message"
"sync/atomic"
"testing"
)
@ -35,7 +37,56 @@ func TestMsg(t *testing.T) {
}
func TestBytes(t *testing.T) {
s := "90007F8181B60B10183280003FFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFF"
d, _ := hex.DecodeString(s)
fmt.Println(len(d))
dd := make([]byte, 0)
dd = append(dd, 0)
dd = append(dd, 1)
dd = append(dd, 2)
dd = append(dd, 3)
dd = append(dd, 4)
fmt.Println(dd[1:])
}
func TestArr(t *testing.T) {
data := make([]byte, 0)
data = append(data, 1)
dd := &message.BtmVobcMsgFree{BtmStatus: message.BTM_STSTUS_NORMAL, WorkTemperature: 10, Fun1: uint16(0), Fun2: uint16(0), Fun3: uint16(0), Fun4: uint16(0),
FreeMsg: data, RespTime: byte(3), VobcLifeNum: 2, BaseBtmVobc: message.BaseBtmVobc{AutoIdFrame: 1}}
s, _ := json.Marshal(dd)
fmt.Println(string(s))
dd2 := &message.BtmVobcMsgFree{}
json.Unmarshal(s, dd2)
fmt.Println(dd2)
}
func TestRes(t *testing.T) {
ss := "fffee601804f004d00e40321002a004fd6c86f3c2bcde891220700000000000000000000000000002103e20000000000000000000052bed1a5e9edfffd"
ss = "fffee601804f004d006a1521002a00f3329169ec2f9c9991229e000000000000000000000000000021156800000000000000000000bff47aff004e24fffd"
ss = "fffee601804f004d00cd1921002a005ea577ff00db4b628891220600000000000000000000000000002119cb0000000000000000000077ba3334d06ffffd"
ss = "fffee601804f004d006c1d21002a00e2c732ff00f86276339122a80000000000000000000000000000211d6a0000000000000000000049c46daebcf6fffd"
ss = "fffee601804f004d00871d21002a00ee97d0fb1c4b2a849122c30000000000000000000000000000211d8500000000000000000000c4e2343e4117fffd"
ss = "fffee601804f004d00444a1c002a00473bbfa11d4b3b029122a800000000000000000000000000001c4a42000000000000000000008970492fff00d0fffd"
//ss = "fffee601804f004d00921d21002a00ea014fef63995bff009122ce0000000000000000000000000000211d90000000000000000000007b07310379cefffd"
cfs, _ := hex.DecodeString(ss)
frameType, dataText, _ := message.BtmVobcDecode(cfs)
if frameType == message.COMMAND_TYPE {
fmt.Println("1111111111111111")
} else if frameType == message.REQUEST_TYPE {
req := &message.BtmVobcReq{}
req.Decode(dataText)
fmt.Println("22222222222222222222")
} else {
fmt.Println("zzzzzzzzzzzzzzzzzzzzz")
}
}
func TestRes3(t *testing.T) {
ss := "fffef890141175e15511001c4a3b0000000000fd5956a5fffd"
arr, _ := hex.DecodeString(ss)
_, dataText, err := message.BtmVobcDecode(arr)
if err != nil {
fmt.Println(err.Error())
}
req := &message.BtmVobcIdCommand{}
req.Decode(dataText)
}

8
third_party/can_btm/balise_btm.go vendored
View File

@ -93,7 +93,7 @@ type BtmCanetClient interface {
Start(bcm BtmCanetManager)
Stop()
//HandleTrainHeadPositionInfo 处理收到列车位置信息
HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo)
HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo)
//获取BTM显示状态 + btm最新的状态
GetState() state_proto.BTMState
}
@ -104,8 +104,8 @@ var (
)
func Default() BtmCanetClient {
btmClientLocker.Lock()
defer btmClientLocker.Unlock()
btmClientLocker.Lock()
if btmClient == nil {
btmClient = &btmCanetClient{baliseDetector: &BaliseDetector{}}
}
@ -113,11 +113,11 @@ func Default() BtmCanetClient {
}
// HandleTrainHeadPositionInfo 处理来自动力学的列车位置信息
func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, h *TrainHeadPositionInfo) {
func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo) {
//slog.Debug(h.String())
wd := entity.GetWorldData(w)
repo := wd.Repo
s.baliseDetector.detect(wd, repo, h)
s.baliseDetector.detect(wd, repo, h, vobcBtm)
}
func (s *btmCanetClient) Start(bcm BtmCanetManager) {
s.bcm = bcm

51
third_party/can_btm/balise_detection.go vendored
View File

@ -2,6 +2,8 @@ package can_btm
import (
"fmt"
"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"
@ -33,15 +35,17 @@ const (
// TrainHeadPositionInfo 列车车头运行位置信息
type TrainHeadPositionInfo = fi.TrainHeadPositionInfo
type BtmAntennaToBaliseInfo struct {
Distance int64 //BTM天线中心到应答器的距离，mm
BaliseId string //应答器id
Distance int64 //BTM天线中心到应答器的距离，mm
BaliseId string //应答器id
BaliseType proto.Transponder_Type //应答器类型
}
type BtmAntennaScanningBaliseInfo struct {
BaliseId string //应答器id
Time time.Time //应答器预计被BTM天线激活的时刻
active bool //true-激活过，即列车扫过
telegram []byte //应答器报文
Distance int64 //BTM天线中心到应答器的距离，mm
BaliseId string //应答器id
Time time.Time //应答器预计被BTM天线激活的时刻
active bool //true-激活过，即列车扫过
telegram []byte //应答器报文
Distance int64 //BTM天线中心到应答器的距离，mm
BaliseType proto.Transponder_Type //应答器类型
}
// BaliseDetector 车载BTM天线，应答器探测器
@ -72,7 +76,7 @@ func (t *BaliseDetector) tryRebind(th *TrainHeadPositionInfo) {
}
}
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo) {
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
t.tryRebind(th)
//if !t.powerAmplifierSwitch { //天线功率放大器未开启，不进行探测
// return
@ -82,15 +86,21 @@ func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Reposi
curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th)
//预测BTM天线到最近一个应答器的时刻
curExpect := t.timeScanNearestBalise(curTime, wd, repo, curAntennaRi)
if curExpect != nil && curExpect.Time.UnixMilli()-curTime.UnixMilli() < 20 { //20ms
//slog.Debug("将要激活应答器", "BaliseId", curExpect.BaliseId, "ActiveTime", dt)
telegram := 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() //应答器计数器
telegram := t.rcvTelegram(wd, curExpect.BaliseId)
if len(telegram) > 0 {
curExpect.telegram = telegram
t.baliseMessageCounterAdd1() //报文计数器
}
}
//BTM天线即将经过应答器
@ -98,6 +108,17 @@ func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Reposi
} else {
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]
@ -186,7 +207,7 @@ func (t *BaliseDetector) timeScanNearestBalise(curTime time.Time, wd *component.
s := float64(expectedBalise.Distance) / 1000
st, ok := t.calculateBtmAntennaScanNextBaliseTime(curTime, curV, curAc, s)
if ok {
return &BtmAntennaScanningBaliseInfo{BaliseId: expectedBalise.BaliseId, Time: st, Distance: expectedBalise.Distance}
return &BtmAntennaScanningBaliseInfo{BaliseId: expectedBalise.BaliseId, Time: st, Distance: expectedBalise.Distance, BaliseType: expectedBalise.BaliseType}
}
}
return nil
@ -215,19 +236,23 @@ func (t *BaliseDetector) findBaliseWillScanByBtmAntenna(wd *component.WorldData,
rs1 := t.searchBalisesFromLinkPosition(repo, ba.LinkId, ba.Up, ba.LinkOffset)
if ba.Up {
if len(rs1) > 0 {
return &BtmAntennaToBaliseInfo{BaliseId: rs1[0].Id(), Distance: rs1[0].LinkPosition().Offset() - ba.LinkOffset}
rs := rs1[0]
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: rs.LinkPosition().Offset() - ba.LinkOffset, BaliseType: rs.BaliseType()}
} else {
nextLinkPort := t.getNextLink(wd, repo, ba.LinkId, ba.Up)
if nextLinkPort != nil {
if nextLinkPort.IsPortA() {
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), true, 0)
if len(rs2) > 0 {
return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: baLink.Length() - ba.LinkOffset + rs2[0].LinkPosition().Offset()}
rs := rs2[0]
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
}
} else {
rs2 := t.searchBalisesFromLinkPosition(repo, nextLinkPort.Link().Id(), false, nextLinkPort.Link().Length())
if len(rs2) > 0 {
return &BtmAntennaToBaliseInfo{BaliseId: rs2[0].Id(), Distance: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs2[0].LinkPosition().Offset()}
rs := rs2[0]
return &BtmAntennaToBaliseInfo{BaliseId: rs.Id(), Distance: baLink.Length() - ba.LinkOffset + nextLinkPort.Link().Length() - rs.LinkPosition().Offset(), BaliseType: rs.BaliseType()}
}
}
}

552
third_party/message/btm_vobc_data.go vendored
View File

@ -32,6 +32,7 @@ var PACKAGE_END = []byte{0xFF, 0xFD}
func BtmVobcDecode(packData []byte) (byte, []byte, error) {
head := packData[:2]
end := packData[len(packData)-2:]
if !bytes.Equal(head, PACKAGE_HEAD) || !bytes.Equal(end, PACKAGE_END) {
slog.Error("btm vobc 数据包头或包尾尾错误", hex.EncodeToString(packData))
return 0, nil, fmt.Errorf("btm vobc 数据包头或包尾尾错误")
@ -43,16 +44,386 @@ func BtmVobcDecode(packData []byte) (byte, []byte, error) {
return 0, nil, fmt.Errorf("btm vobc 解析未知命令帧")
}
dataText := data[1:]
frameType := dataText[0]
return frameType, dataText, nil
/* if frameType == COMMAND_TYPE {
if command == ID_COMMAND_TYPE {
} else if frameType == REQUEST_TYPE {
frameType := dataText[0]
return frameType, dataText, nil
} else if command == REQT_TYPE {
newDatatext, _ := TranslateFromFFFE(dataText)
rssp := &RsspRsd{}
rsspErr := rssp.Decode(newDatatext)
if rsspErr != nil {
//slog.Error("解析请求帧rssp-i失败,元数据:", hex.EncodeToString(cfs), "错误信息:", rsspErr.Error())
return 0, nil, fmt.Errorf("解析请求帧rssp-i失败")
}
return rssp.Sad[0], rssp.Sad, nil
}
return 0, nil, fmt.Errorf("btm vobc 解析未知命令帧")
}
const (
WAIT_FF_C1 = 0x00
WAIT_FF_C2 = 0x01
WAIT_NO_FF = 0x02
WAIT_FF_POS = 0x03
ABNORMAL_STATE = 0x04
/*FFFE数据接收状态*/
COM_WAIT_START_FF = 0x00
COM_WAIT_START_FE = 0x01
COM_WAIT_STOP_FF = 0x02
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++
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 {
slog.Error("btm vobc 解析未知命令帧类型", strconv.FormatInt(int64(frameType), 16))
return
}*/
/*尚未遇到前一个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
}
// 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
}
type BaseBtmVobc struct {
@ -62,23 +433,97 @@ type BaseBtmVobc struct {
Crc32 uint32
}
func baseDecode(d []byte) (BaseBtmVobc, []byte) {
frame := d[0]
frameLen := d[1]
autoIdFrame := d[2]
func baseDecode(d []byte) (BaseBtmVobc, *bytes.Buffer) {
buf := bytes.NewBuffer(d)
frame, _ := buf.ReadByte()
frameLen, _ := buf.ReadByte()
autoIdFrame, _ := buf.ReadByte()
crc32 := binary.BigEndian.Uint32(d[len(d)-4:])
realData := d[3 : len(d)-4]
return BaseBtmVobc{Frame: frame, FrameLen: frameLen, Crc32: crc32, AutoIdFrame: autoIdFrame}, realData
return BaseBtmVobc{Frame: frame, FrameLen: frameLen, Crc32: crc32, AutoIdFrame: autoIdFrame}, buf
}
func baseEncode(source []byte) []byte {
data := make([]byte, 0)
crc32 := crc.CalculateCRC(crc.CRC32, source)
newSource := binary.BigEndian.AppendUint32(source, uint32(crc32))
//fffeData, _ := TranslateToFFFE(newSource)
//fffeData := TranslateToFFFE2(newSource)
//fffeData := FFFEEncode(newSource)
//fffeData, _ := TranslateToFFFE3(newSource, uint16(len(newSource)))
/*f2 := make([]byte, len(newSource)*2)
lens, _ := TranslateToFFFE4(newSource, f2)
fffeData := f2[:lens]*/
f2 := make([]byte, len(newSource)*2)
lens := aa(newSource, f2)
fffeData := f2[:lens]
data = append(data, PACKAGE_HEAD...)
data = append(data, newSource...)
data = append(data, fffeData...)
data = append(data, PACKAGE_END...)
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 {
@ -90,13 +535,29 @@ type BtmVobcIdCommand struct {
}
func (b *BtmVobcIdCommand) Decode(data []byte) {
base, realData := baseDecode(data)
base, buf := baseDecode(data)
b.BaseBtmVobc = base
b.BtmId = binary.BigEndian.Uint16(realData[0:2])
b.VobcId = binary.BigEndian.Uint16(realData[2:4])
b.VobcLifeNum = binary.BigEndian.Uint32(realData[4:8])
var btmId uint16
var vobcId uint16
var lifeNum uint32
binary.Read(buf, binary.BigEndian, &btmId)
binary.Read(buf, binary.BigEndian, &vobcId)
binary.Read(buf, binary.BigEndian, &lifeNum)
b.BtmId = btmId
b.VobcId = vobcId
b.VobcLifeNum = lifeNum
}
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
)
// 请求帧的正文
type BtmVobcReq struct {
BaseBtmVobc
@ -110,17 +571,34 @@ type BtmVobcReq struct {
VobcLifeWalkDistance uint16 //VOBC 周期走行距离 单位：cm
}
const (
a = 0x06
b = 0x15
)
func (b *BtmVobcReq) Decode(data []byte) {
base, realData := baseDecode(data)
base, buf := baseDecode(data)
b.BaseBtmVobc = base
b.FrameStatus = realData[0]
b.MessageType = realData[1]
b.MessageSerial = realData[2]
b.yuliu = realData[3:13]
b.VobcLifeNum = binary.BigEndian.Uint32(realData[13:17])
b.TimeStamp = realData[17:25]
b.Speed = binary.BigEndian.Uint16(realData[25:26])
b.VobcLifeWalkDistance = binary.BigEndian.Uint16(realData[26:27])
b.FrameStatus, _ = buf.ReadByte()
b.MessageType, _ = buf.ReadByte()
b.MessageSerial, _ = buf.ReadByte()
var tyuli = make([]byte, 10)
buf.Read(tyuli)
//b.yuliu = data[5:15]
var lifeNum uint32
ts := make([]byte, 6)
var speed uint16
var walkDis uint16
binary.Read(buf, binary.BigEndian, &lifeNum)
buf.Read(ts)
binary.Read(buf, binary.BigEndian, &speed)
binary.Read(buf, binary.BigEndian, &walkDis)
b.VobcLifeNum = lifeNum
b.TimeStamp = ts
b.Speed = speed
b.VobcLifeWalkDistance = walkDis
}
var MESSAGE_AUTO_ID byte = 1
@ -139,8 +617,7 @@ func getAutoId() byte {
return MESSAGE_AUTO_ID
}
func getAutoMessageId() byte {
func GetAutoMessageId() byte {
defer lock.Unlock()
lock.Lock()
if MESSAGE_SERIAL <= 0 {
@ -159,6 +636,11 @@ const (
var mesage_yuliu = []byte{0, 0, 0, 0, 0, 0, 0, 0, 0}
const (
BTM_STSTUS_NORMAL = 0x00
BTM_STSTUS_WARN = 0x04
)
// 报文帧的正文
type BtmVobcMessage struct {
BaseBtmVobc
@ -179,11 +661,11 @@ func (b *BtmVobcMessage) Encode() []byte {
data := make([]byte, 0)
buf := bytes.NewBuffer(data)
b.MsgSerial = getAutoMessageId()
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, getAutoId())
binary.Write(buf, binary.BigEndian, b.FontTtl)
binary.Write(buf, binary.BigEndian, b.MsgSerial)
binary.Write(buf, binary.BigEndian, b.BtmStatus)
@ -247,11 +729,11 @@ type BtmVobcMsgFree struct {
func (b *BtmVobcMsgFree) Encode() []byte {
data := make([]byte, 0)
buf := bytes.NewBuffer(data)
b.MsgSerial = getAutoMessageId()
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, getAutoId())
//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)

24
ts/simulation/wayside/memory/wayside_memory_train.go
View File

@ -105,7 +105,7 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
//status.TrainActiveDirection = trainActDir
status.Tcc = initTrainTcc(vs, status.TrainRunUp)
status.VobcBtm = &state_proto.VobcBtmState{TelegramState: make([]*state_proto.VobcBtmState_TelegramState, 0), History: make(map[uint32]*state_proto.VobcBtmState_VobcBtmHistoryState)}
slog.Debug("列车初始化", "trainIndex", trainIndex, "linkId", linkId, "loffset", loffset)
linkIdInt, _ := strconv.Atoi(linkId)
err := dynamics.Default().RequestAddTrain(&message.InitTrainInfo{
@ -308,9 +308,9 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
}
//runDirection 指定的是link方向
//pointTO 指的是是否ab，或是否到岔心
runDirection, pointTo := QueryDirectionAndABByDevice(vs.Repo, id, port, info.Up)
_, pointTo := QueryDirectionAndABByDevice(vs.Repo, id, port, info.Up)
slog.Debug("处理动力学转换后的消息", "number", info.Number, "up", info.Up, "Link", info.Link, "车头位置", id, "偏移", offset, "是否上行", runDirection, "是否ab", pointTo, "t1Dir:", info.TrainActToMax, "t2Dir:", info.TrainActToMin)
//slog.Debug("处理动力学转换后的消息", "number", info.Number, "up", info.Up, "Link", info.Link, "车头位置", id, "偏移", offset, "是否上行", runDirection, "是否ab", pointTo, "t1Dir:", info.TrainActToMax, "t2Dir:", info.TrainActToMin)
trainHeadActUp := true
if info.TrainActToMax || info.TrainActToMin {
if info.TrainActToMin {
@ -368,6 +368,9 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
return sta
}
// 根据动力学列车激活端方向，返回前端列车激活端
// 道岔 1是端口到岔心 2是岔心到端口
// 区段 1是a-b 2是b-a
func updateTrainActiveDirFromDynamic(vs *VerifySimulation, info *message.DynamicsTrainInfo, sta *state_proto.TrainState, id, port string, trainHeadActUp bool) {
sta.TrainActiveDirection = 0
if info.TrainActToMax || info.TrainActToMin {
@ -384,7 +387,7 @@ func updateTrainActiveDirFromDynamic(vs *VerifySimulation, info *message.Dynamic
// 根据列车位置修改列车应答器
func updateTrainBtmPosition(vs *VerifySimulation, info *message.DynamicsTrainInfo, sta *state_proto.TrainState, outLinkId string, outLinkOffset int64) {
// 更新BTM中列车位置信息
can_btm.Default().HandleTrainHeadPositionInfo(vs.World, &fi.TrainHeadPositionInfo{
can_btm.Default().HandleTrainHeadPositionInfo(vs.World, sta.VobcBtm, &fi.TrainHeadPositionInfo{
TrainId: sta.Id,
Up: info.Up,
Link: outLinkId,
@ -393,6 +396,7 @@ func updateTrainBtmPosition(vs *VerifySimulation, info *message.DynamicsTrainInf
Acceleration: info.Acceleration,
})
state := can_btm.Default().GetState()
//fmt.Println(state.Telegram)
sta.BtmState = &state
}
@ -509,6 +513,7 @@ func RemoveTrainState(vs *VerifySimulation, id string) {
if ok {
t := d.(*state_proto.TrainState)
err := removeTrain(vs, id, t)
clearTrainVobcBtmState(vs, id)
if err != nil {
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: err.Error()})
}
@ -526,3 +531,14 @@ func calcTrailTailOffset(headerOffset, length int64, up bool) (calctailOffset in
}
return
}
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
}

10
ts/simulation/wayside/memory/wayside_simulation_train_pc.go
View File

@ -248,7 +248,7 @@ func (s *VerifySimulation) findConnTrain2(ct1, ct2 state_proto.TrainConnState_Tr
})
return trains
}
func (s *VerifySimulation) findConnTrain(ct state_proto.TrainConnState_TrainConnType) *state_proto.TrainState {
func (s *VerifySimulation) FindConnTrain(ct state_proto.TrainConnState_TrainConnType) *state_proto.TrainState {
var findTrain *state_proto.TrainState
s.Memory.Status.TrainStateMap.Range(func(k, v any) bool {
train := v.(*state_proto.TrainState)
@ -264,7 +264,7 @@ func (s *VerifySimulation) findConnTrain(ct state_proto.TrainConnState_TrainConn
// 4.4.1. 车载输出数字量信息报文内容
func (s *VerifySimulation) TrainPcSimDigitalOutInfoHandle(connType state_proto.TrainConnState_TrainConnType, data []byte) {
train := s.findConnTrain(connType)
train := s.FindConnTrain(connType)
if train == nil {
slog.Error("车载输出数字量，未找到连接车载pc仿真的列车")
return
@ -432,7 +432,7 @@ func trainPcSimDigitalOutInfoHandleCode7_0(d byte, vobc *state_proto.TrainVobcSt
// 4.4.2. 车载输出数字反馈量信息报文内容
func (s *VerifySimulation) TrainPcSimDigitalReportHandle(connType state_proto.TrainConnState_TrainConnType, data []byte) {
train := s.findConnTrain(connType)
train := s.FindConnTrain(connType)
if train == nil {
slog.Error("车载输出数字反馈量信息,未找到连接车载pc仿真的列车")
return
@ -495,7 +495,7 @@ func TrainPcSimConnOrRemoveHandle(train *state_proto.TrainState) error {
// 4.4.3. 车载输出模拟量信息报文内容(0x03)
func (s *VerifySimulation) TrainPcSimMockInfo(connType state_proto.TrainConnState_TrainConnType, data []byte) {
train := s.findConnTrain(connType)
train := s.FindConnTrain(connType)
if train == nil {
slog.Error("车载输出模拟量,未找到连接车载pc仿真的列车")
return
@ -514,7 +514,7 @@ func (s *VerifySimulation) TrainBtmQuery(connType state_proto.TrainConnState_Tra
slog.Error("列车btm查询报文长度错误:", len(data))
return
}
train := s.findConnTrain(connType)
train := s.FindConnTrain(connType)
if train == nil {
slog.Error("车载输出btm查询,未找到连接车载pc仿真的列车")
return