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

Merge remote-tracking branch 'origin/develop' into develop

Browse Source 
# Conflicts:
#	rts-sim-module
This commit is contained in:
thesai 2024-08-07 14:54:37 +08:00
commit 844560ae1e
31 changed files with 2719 additions and 1231 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
api/simulation.go
View File

@ -246,6 +246,7 @@ func addTrain(c *gin.Context) {
Id: fmt.Sprintf("%v", req.TrainId),
HeadDeviceId: req.Id,
HeadOffset: req.HeadOffset,
//HeadOffset: 93211,
DevicePort: req.DevicePort,
TrainRunUp: req.RunDirection,
//RunDirection: req.RunDirection,

43
bin/acc_conn-example.go
View File

@ -1,7 +1,6 @@
package main
import (
"encoding/binary"
"encoding/hex"
"fmt"
"github.com/spf13/viper"
@ -79,11 +78,6 @@ func initTrainPc() {
//sendBtm()
}
const (
e1 = 0x1e
aa = 0x17
)
func createOrRemoveTrain() {
msgs := make([]*message.TrainPcSimBaseMessage, 0)
//msgs = append(msgs, &message.TrainPcSimBaseMessage{Data: []byte{0x00}, Type: message.RECIVE_TRAIN_CREATE_REMOVE})
@ -146,45 +140,10 @@ func circleSendTrainMockData() {
time.Sleep(time.Millisecond * 1000)
}
}
func sendBtm() {
source := "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"
data, _ := hex.DecodeString(source)
trainpcClient.Send(data)
}
func circleSendTrainSpeedPlace() {
for {
data := make([]byte, 0)
data = binary.BigEndian.AppendUint16(data, uint16(2))
//data = binary.BigEndian.AppendUint16(data, uint16(1))
//data = binary.BigEndian.AppendUint16(data, uint16(0))
//data = binary.BigEndian.AppendUint16(data, uint16(0))
//data = binary.BigEndian.AppendUint32(data, uint32(940))
//data = binary.BigEndian.AppendUint32(data, uint32(940))
data = binary.BigEndian.AppendUint32(data, uint32(0))
data = binary.BigEndian.AppendUint32(data, uint32(0))
data = binary.BigEndian.AppendUint32(data, uint32(940))
data = binary.BigEndian.AppendUint32(data, uint32(940))
now := time.Now().UTC()
sec := now.Unix()
data = binary.BigEndian.AppendUint32(data, uint32(27797))
data = binary.BigEndian.AppendUint16(data, uint16(now.UnixMilli()-sec*1000))
ss := "eb011d000200000000000000000000000000000000000017af0299ffff"
data, _ = hex.DecodeString(ss)
//bm := &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_LOCATION_INFO, Data: data}
//dataCode := bm.Encode()
//fmt.Println(fmt.Sprintf("发送列车位置信息:%v", hex.EncodeToString(data)))
trainpcClient.Send(data)
//dd := &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{3, 0}}
//trainpcClient.Send(dd.Encode())
time.Sleep(time.Millisecond * 80)
}
}
func trainPcDataHandle(n int, data []byte) {
hexData := hex.EncodeToString(data[:n])
slog.Info(fmt.Sprintf("列车pc仿真接口长度:%v,实际长度:%v,接受数据:%v", len(data), n, hexData))
//source := "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"
//data2, _ := hex.DecodeString(source)
//trainpcClient.Send(data2)
}
func trainPcConnErr(err error) {

8
config/dev.yml
View File

@ -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

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

@ -3222,8 +3222,8 @@ type Transponder struct {
FixedTelegram string `protobuf:"bytes,11,opt,name=fixedTelegram,proto3" json:"fixedTelegram,omitempty"` //应答器固定报文
FixedUserTelegram string `protobuf:"bytes,12,opt,name=fixedUserTelegram,proto3" json:"fixedUserTelegram,omitempty"` //应答器固定用户报文
OriginalCode string `protobuf:"bytes,13,opt,name=originalCode,proto3" json:"originalCode,omitempty"` //应答器原编号（厂商提供数据编号）
LeuIndex uint32 `protobuf:"varint,14,opt,name=leuIndex,proto3" json:"leuIndex,omitempty"` //LEU索引
LeuInsideIndex uint32 `protobuf:"varint,15,opt,name=leuInsideIndex,proto3" json:"leuInsideIndex,omitempty"` // LEU内部索引
LeuIndex uint32 `protobuf:"varint,14,opt,name=leuIndex,proto3" json:"leuIndex,omitempty"` //应答器所属LEU的索引
LeuInsideIndex uint32 `protobuf:"varint,15,opt,name=leuInsideIndex,proto3" json:"leuInsideIndex,omitempty"` // 应答器在LEU内部的索引
}
func (x *Transponder) Reset() {

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

File diff suppressed because it is too large Load Diff

406
example/balise_1023_830/main.go Normal file
View File

@ -0,0 +1,406 @@
package main
import (
"fmt"
"log/slog"
"strconv"
"joylink.club/bj-rtsts-server/third_party/balise"
)
type Transponder struct {
// 应答器编号
Code int
// 应答器名称
Name string
// 应答器类型
Type string
// 校验码
CheckSum string
// 830报文
Msg830 string
// 1023报文
Msg1023 string
}
func main() {
transponderMap := ReadTransponders()
// v := transponderMap["FB121_BGZ"]
// b1023 := convertToBalise1023(v.Msg1023)
// replaced830 := replaceFirst10Bits(convertTo830(v.Msg830))
// scrambled830 := scrambling(replaced830, b1023.S)
// compare830(scrambled830, b1023.data830)
// count := 0
for _, v := range transponderMap {
// slog.Info("transponder", "name", v.Name, "code", v.Code, "checksum", v.CheckSum, "msg830", v.Msg830, "msg1023", v.Msg1023)
byte104, err := balise.DecodeByteString(v.Msg1023)
if err != nil {
slog.Error("解码应答器数据失败", "name", v.Name, "code", v.Code, "err", err)
continue
}
compare104Bytes(byte104, balise.ConvertByteStringToBytes(v.Msg830))
// b1023 := convertToBalise1023(v.Msg1023)
// source830 := convertTo830(v.Msg830)
// // replaced830 := replaceFirst10Bits(source830)
// // compare830(reverted830, source830)
// descrambled830 := descrambling(b1023.data830, b1023.S)
// reverted830 := revertFirst10Bits(descrambled830)
// // // scrambled830 := scrambling(replaced830, b1023.S)
// compare830(source830, reverted830)
// count++
// if count >= 10 {
// break
// }
}
}
// 比较830位数据
func compare104Bytes(bytes104 []byte, compare104 []byte) {
if len(bytes104) != 104 {
panic("invalid length")
}
for i := 0; i < 104; i++ {
fmt.Printf("%02x\n", bytes104[i])
fmt.Printf("%02x\n\n", compare104[i])
if bytes104[i] != compare104[i] {
slog.Info("error", "index", i, "bytes104", fmt.Sprintf("%02x", bytes104[i]), "compare", fmt.Sprintf("%02x", compare104[i]))
panic("104 bytes compare error")
}
}
}
type Balise1023 struct {
bits []byte // 1023报文,1023bit
data []byte // 数据位,913bit
data830 []byte // 830位数据
cb []byte // 控制位,3bit
sb []byte // 加扰位,12bit
S uint32 // 由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
esb []byte // 额外修正位,10bit
checkSum []byte // 校验位,85bit
}
func newBalise1023(bits []byte) *Balise1023 {
if len(bits) != 1023 {
panic("invalid length")
}
// for i := 0; i < 1023; i++ {
// if i%10 == 0 {
// println()
// }
// print(1022-i, ":", bits[i], ",")
// }
// println()
balise1023 := &Balise1023{
bits: bits,
}
balise1023.data = balise1023.getRange(1022, 110)
if len(balise1023.data) != 913 {
panic("invalid data length")
}
balise1023.data830 = convert913To830(balise1023.data)
balise1023.check11To10()
balise1023.cb = balise1023.getRange(109, 107)
for i, v := range balise1023.cb {
n := 109 - i
name := "b" + strconv.Itoa(109-i)
slog.Info("cb", name, v)
if n == 109 && v == 1 {
slog.Error("控制位cb错误：cb109应该为0，实际为1")
} else if n == 108 && v == 1 {
slog.Error("控制位cb错误：cb108应该为0，实际为1")
} else if n == 107 && v == 0 {
slog.Error("控制位cb错误：cb107应该为1，实际为0")
}
}
balise1023.sb = balise1023.getRange(106, 95)
balise1023.S = calculateS(balise1023.sb)
balise1023.esb = balise1023.getRange(94, 85)
balise1023.checkSum = balise1023.getRange(84, 0)
slog.Info("msg length", "datalen", len(balise1023.data), "cblen", len(balise1023.cb), "sblen", len(balise1023.sb), "esblen", len(balise1023.esb), "checkSumlen", len(balise1023.checkSum))
return balise1023
}
func (b *Balise1023) getRange(start, end int) []byte {
if start < 0 || end < 0 || start < end || start > 1022 {
panic("invalid range")
}
return b.bits[1022-start : 1022-(end-1)]
}
func (b *Balise1023) check11To10() {
b913 := b.data
compare := convert830To913(b.data830)
for i := 0; i < 913; i++ {
if b913[i] != compare[i] {
slog.Info("error", "idx", i, "b913", b913[i], "compare", compare[i])
panic("10 to 11 bit error")
}
}
}
// 转换字节字符串到bit数组，左边为最高有效位（MSB）
func convertStringBytesToBits(msg string) []byte {
length := len(msg)
println("msg:", msg)
bytes := make([]byte, length/2)
for i := 0; i < length; i += 2 {
v, err := strconv.ParseUint(msg[i:i+2], 16, 8)
if err != nil {
panic(err)
}
bytes[i/2] = byte(v)
// slog.Info("i", "byteidx", i/2, "byte", fmt.Sprintf("%02x", v))
}
// 字节转换为bit数组
bits := make([]byte, length/2*8)
for i, bt := range bytes {
for j := 0; j < 8; j++ {
move := 7 - j
idx := i*8 + j
bits[idx] = (bt >> move) & 1
}
}
return bits
}
func convertTo830(msg string) []byte {
length := len(msg)
if length != 208 {
panic("invalid length")
}
// 字节转换为bit数组
bits := convertStringBytesToBits(msg)
bits830 := bits[0:830]
return bits830
}
func convertToBalise1023(msg string) *Balise1023 {
length := len(msg)
if length != 256 {
panic("invalid length")
}
// 字节转换为bit数组
bits := convertStringBytesToBits(msg)
bits1023 := bits[0:1023]
slog.Info("bits length", "len", len(bits1023))
return newBalise1023(bits1023)
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组，除了第一个10位值，其余值求和，然后循环2的10次方次与其他值求和结果相加后模2的10次方，若结果和第一个10位值相同，则结束，此值即为原始的第一个10位值，将此值替换为第一个10位二进制数组，依然是左边为MSB
func revertFirst10Bits(b []byte) []byte {
if len(b) != 830 {
panic("invalid length")
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组
bits := make([]uint16, 83)
for i := 0; i < 83; i++ {
bits[i] = uint16(balise.ToValLeftMsb(b[i*10 : i*10+10]))
// 打印输出
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b[i*10+j])
}
print(" ")
if i != 0 && i%10 == 9 {
println()
}
}
println()
// 将除了第一个10位字整数求和
sum := uint64(0)
for i := 1; i < 83; i++ {
sum += uint64(bits[i])
}
// 循环2的10次方次与其他值求和结果相加后模2的10次方
for i := 0; i < 1024; i++ {
test := sum + uint64(i)
if test%1024 == uint64(bits[0]) {
bits[0] = uint16(i)
break
}
}
slog.Info("还原第一个10位值", "sum", sum, "bits[0]", bits[0], "bits[0]b", fmt.Sprintf("%010b", bits[0]))
rbits := make([]byte, 830)
// 将整个10位数组转换为二进制数组，依然是MSB
u0bits := balise.ToBitsLeftMsb(int(bits[0]), 10)
for i := 0; i < 10; i++ {
rbits[i] = u0bits[i]
}
for i := 10; i < 830; i++ {
rbits[i] = b[i]
}
// compare830(b, rbits)
return rbits
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组，然后求和后模2的10次方，得到的结果覆盖第一个10位值，然后将整个10位数组转换为二进制数组，依然是左边为MSB
func replaceFirst10Bits(b []byte) []byte {
if len(b) != 830 {
panic("invalid length")
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组
bits := make([]uint16, 83)
for i := 0; i < 83; i++ {
bits[i] = uint16(balise.ToValLeftMsb(b[i*10 : i*10+10]))
// 打印输出
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b[i*10+j])
}
print(" ")
if i != 0 && i%10 == 9 {
println()
}
}
println()
// 将每一个10位字整数求和后模2的10次方，得到的结果覆盖第一个10位值
sum := uint64(0)
for i := 0; i < 83; i++ {
sum += uint64(bits[i])
// fmt.Printf("i=%d, v10=%d, v10b=%010b\n", i, bits[i], bits[i])
}
bits[0] = uint16(sum % 1024)
slog.Info("替换第一个10位值", "sum", sum, "bits[0]", bits[0], "bits[0]b", fmt.Sprintf("%010b", bits[0]))
rbits := make([]byte, 830)
// 将整个10位数组转换为二进制数组，依然是MSB
u0bits := balise.ToBitsLeftMsb(int(bits[0]), 10)
for i := 0; i < 10; i++ {
rbits[i] = u0bits[i]
}
for i := 10; i < 830; i++ {
rbits[i] = b[i]
}
// compare830(b, rbits)
return rbits
}
// 由加扰位计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
func calculateS(sb []byte) uint32 {
// 由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
if len(sb) != 12 {
panic("invalid length")
}
B := balise.ToValLeftMsb(sb)
const A uint64 = 2801775573
S := uint32((A * uint64(B)) % (1 << 32))
slog.Info("由12位加扰位计算得到整数S", "B", B, "S", S, "Sb", fmt.Sprintf("%032b", S))
return S
}
// 由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
// 1. 生成一个32位的线性反馈移位寄存器，其初始状态为S（左边为MSB）
// 2. 系数h31,h30,h29,h27,h25和h0等于1(表示连接)，所有其他系数都为0（表示不连接）
// 3. 然后电路被时钟驱动m-1次，其中m是数据位的数量，同时输入dn的每一位dn(m-1),dn(m-2),...,dn(0),便生成加扰后的码位（在第一个时钟之前读取第一个输出out(m-1)）
// 4. 生成的加扰码位是dn的每一位与S的最高位的异或值
// 5. 生成的加扰码位会根据系数进行异或反馈回S的最低位
// 几种可能性：
func scrambling(dn []byte, S uint32) []byte {
if len(dn) != 830 {
panic("invalid length")
}
// const Polynomial = 0x000000AF
out := make([]byte, len(dn))
t := S // 寄存器初始值
for i := 0; i < len(dn); i++ {
msb := (t >> 31) & 1
out[i] = (dn[i] ^ byte(msb)) & 1
// fmt.Printf("i=%d, t=%032b, msb=%d, dn=%d, out=%d\n", i, t, msb, dn[i], out[i])
xor := uint32(out[i])
t = (xor << 30) ^ (xor << 29) ^ (xor << 28) ^ (xor << 26) ^ (xor << 24) ^ t
t = (t << 1) | xor
}
return out
}
func descrambling(dn []byte, S uint32) []byte {
if len(dn) != 830 {
panic("invalid length")
}
// const Polynomial = 0x000000AF
out := make([]byte, len(dn))
t := S // 寄存器初始值
for i := 0; i < len(dn); i++ {
msb := (t >> 31) & 1
out[i] = (dn[i] ^ byte(msb)) & 1
// fmt.Printf("i=%d, t=%032b, msb=%d, dn=%d, out=%d\n", i, t, msb, dn[i], out[i])
xor := uint32(dn[i])
t = (xor << 30) ^ (xor << 29) ^ (xor << 28) ^ (xor << 26) ^ (xor << 24) ^ t
t = (t << 1) | xor
}
return out
}
// 将830位的二进制数组先以10位为一组分别转换为11位并组合
func convert830To913(b830 []byte) []byte {
if len(b830) != 830 {
panic("invalid length")
}
b913 := make([]byte, 913)
for i := 0; i < 83; i++ {
b11 := balise.To11(b830[i*10 : i*10+10])
for j := 0; j < 11; j++ {
b913[i*11+j] = b11[j]
}
}
return b913
}
func convert913To830(b913 []byte) []byte {
if len(b913) != 913 {
panic("invalid length")
}
b830 := make([]byte, 830)
for i := 0; i < 83; i++ {
b10, err := balise.From11(b913[i*11 : i*11+11])
if err != nil {
panic(err)
}
for j := 0; j < 10; j++ {
b830[i*10+j] = b10[j]
}
}
return b830
}
func compare830(b830 []byte, compare830 []byte) {
if len(b830) != 830 {
panic("invalid length")
}
for i := 0; i < 83; i++ {
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b830[i*10+j])
}
println()
for j := 0; j < 10; j++ {
fmt.Printf("%01b", compare830[i*10+j])
}
println()
println()
}
for i := 0; i < 830; i++ {
if b830[i] != compare830[i] {
slog.Info("error", "index", i, "b830", b830[i], "compare", compare830[i])
panic("830 bit compare error")
}
}
}
// 以11位为一组比较两个913位的报文
func compare913(b913 []byte, b1023 *Balise1023) {
if len(b913) != 913 {
panic("invalid length")
}
compare := b1023.data
for i := 0; i < 913; i += 11 {
for j := 0; j < 11; j++ {
print(b913[i+j])
}
println()
for j := 0; j < 11; j++ {
print(compare[i+j])
}
println()
println()
}
}

156
example/balise_1023_830/read_excel.go Normal file
View File

@ -0,0 +1,156 @@
package main
import (
"log/slog"
"runtime/debug"
"strconv"
"strings"
"github.com/xuri/excelize/v2"
)
const (
fileName = "北岗子-应答器报文清单.xlsx"
sheetName = "应答器报文清单"
codeColumn = "应答器编号"
nameColumn = "应答器名称"
typeColumn = "类型"
checkSumColumn = "校验码"
msg830strColumn = "用户报文（830bits）"
msg1023strColumn = "报文（1023bits）"
)
var heads []string
func init() {
initHeads()
}
func initHeads() {
heads = append(heads, codeColumn)
heads = append(heads, nameColumn)
heads = append(heads, typeColumn)
heads = append(heads, checkSumColumn)
heads = append(heads, msg830strColumn)
heads = append(heads, msg1023strColumn)
}
func buildHeadIndex(row []string) map[string]int {
headIdx := make(map[string]int)
for i, column := range row {
column = HandleStringSpace(column)
if column == codeColumn {
headIdx[column] = i
} else if column == nameColumn {
headIdx[column] = i
} else if column == typeColumn {
headIdx[column] = i
} else if column == checkSumColumn {
headIdx[column] = i
} else if column == msg830strColumn {
headIdx[column] = i
} else if column == msg1023strColumn {
headIdx[column] = i
}
}
// 检查headIndex是否完整
if len(headIdx) <= 0 {
return nil
}
checkHeadsIndex(headIdx, fileName, sheetName, heads)
return headIdx
}
func checkHeadsIndex(headIdx map[string]int, fileName, sheetName string, heads []string) {
// 检查headIndex是否完整
for _, v := range heads {
if _, ok := headIdx[v]; !ok {
slog.Error("表头缺失", "文件名", fileName, "SheetName", sheetName, "表头", v)
panic("课时表头缺失")
}
}
}
func ReadTransponders() map[string]Transponder {
return readExcel(fileName, sheetName, readRows)
}
func readExcel[T any](fileName, sheetName string, handle func(rows [][]string) map[string]T) map[string]T {
f, err := excelize.OpenFile(fileName)
if err != nil {
slog.Error("打开表文件异常", "表名", fileName, "error", err)
debug.PrintStack()
panic("打开表文件异常")
}
defer func() {
if err := f.Close(); err != nil {
slog.Error("文件关闭异常", "error", err)
}
}()
rows, err := f.GetRows(sheetName)
if err != nil {
slog.Error("读取Sheet异常", "SheetName", sheetName, "error", err)
panic(err)
}
// fmt.Println(rows)
return handle(rows)
}
func readRows(rows [][]string) map[string]Transponder {
dataMap := make(map[string]Transponder)
var headIdx map[string]int
for _, row := range rows {
if headIdx == nil {
headIdx = buildHeadIndex(row)
// if headIdx != nil {
// slog.Info("读取到表头索引", "文件名", fileName, "表名", sheetName, "索引", headIdx)
// }
} else {
rowSize := len(row)
if rowSize <= 0 {
continue
}
if rowSize <= headIdx[msg1023strColumn] {
// slog.Info("非数据行", "row", row, "rowIndex", i)
continue
}
codeStr := row[headIdx[codeColumn]]
if codeStr == "" {
continue
}
codeStr = HandleStringSpace(codeStr)
code, err := strconv.ParseInt(codeStr, 10, 32)
if err != nil {
slog.Error("应答器编号错误", "编号", codeStr, "error", err)
panic("应答器编号错误")
}
name := row[headIdx[nameColumn]]
if name == "" {
continue
}
name = HandleStringSpace(name)
tp := row[headIdx[typeColumn]]
checkSum := row[headIdx[checkSumColumn]]
msg830 := row[headIdx[msg830strColumn]]
msg1023 := row[headIdx[msg1023strColumn]]
dataMap[name] = Transponder{
Code: int(code),
Name: name,
Type: tp,
CheckSum: checkSum,
Msg830: msg830,
Msg1023: msg1023,
}
}
}
slog.Info("读取结果", "文件名", fileName, "SheetName", sheetName, "总数", len(dataMap))
// fmt.Println(dataMap)
return dataMap
}
func HandleStringSpace(s string) string {
s = strings.ReplaceAll(s, " ", "")
s = strings.ReplaceAll(s, "\n", "")
return s
}

32
example/ex1/main.go Normal file
View File

@ -0,0 +1,32 @@
package main
import "log/slog"
func main() {
a1 := calculateAvgAcc(136960.88, 24.41978)
a2 := calculateAvgAcc(34874, 14.97515)
slog.Info("根据位移和时间计算平均加速度", "通号平均加速度", a1, "动力学平均加速度", a2)
avt1 := calculateAvgAccByV(6.94444, 24.41978)
avt2 := calculateAvgAccByV(6.94444, 14.97515)
slog.Info("根据速度和时间计算平均加速度", "通号平均加速度", avt1, "动力学平均加速度", avt2)
s1 := calculateS(0.28437766432762146, 24.41978)
s2 := calculateS(0.46373090147972107, 14.97515)
slog.Info("根据加速度和时间计算位移", "通号最小位移", s1, "动力学最小位移", s2)
slog.Info("实际位移", "通号实际位移差", 136960.88-s1, "动力学实际位移差", 34874-s2)
}
// 根据位移和时间计算平均加速度（初始速度为0）
// s单位为mm，t单位为s
func calculateAvgAcc(s float32, t float32) float32 {
return 2 * s / (t * t) / 1000
}
// 根据速度和时间计算平均加速度(初始速度为0)
func calculateAvgAccByV(v float32, t float32) float32 {
return v / t
}
// 根据加速度和时间计算位移（初始速度为0）
func calculateS(a float32, t float32) float32 {
return 0.5 * a * t * t * 1000
}

56
example/lfsr/main.go Normal file
View File

@ -0,0 +1,56 @@
package main
import (
"fmt"
"log/slog"
)
func main() {
// fib_lfsr()
galois_lfsr()
}
/* taps: 16 14 13 11; feedback polynomial: x^16 + x^14 + x^13 + x^11 + 1 */
func fib_lfsr() {
startState := uint16(0xACE1)
lfsr := startState
var bit uint16
period := uint64(0)
for {
slog.Info("fib_lfsr", "bit", fmt.Sprintf("%01b", lfsr&1), "lfsr", fmt.Sprintf("%016b", lfsr))
bit = ((lfsr >> 0) ^ (lfsr >> 2) ^ (lfsr >> 3) ^ (lfsr >> 5)) & 1
lfsr = (lfsr >> 1) | (bit << 15)
period++
if period == 15 {
break
}
if lfsr == startState {
break
}
}
println(period)
}
/* taps: 16 14 13 11; feedback polynomial: x^16 + x^14 + x^13 + x^11 + 1 */
func galois_lfsr() {
startState := uint16(0xACE1)
lfsr := startState
var bit uint16
period := uint64(0)
for {
slog.Info("galois_lfsr", "bit", fmt.Sprintf("%01b", lfsr&1), "lfsr", fmt.Sprintf("%016b", lfsr))
bit = lfsr & 1
lfsr >>= 1
if bit == 1 {
lfsr ^= 0xB400
}
period++
// if period == 15 {
// break
// }
if lfsr == startState {
break
}
}
println(period)
}

52
example/test/main.go Normal file
View File

@ -0,0 +1,52 @@
package main
import (
"fmt"
"math"
)
func main() {
d2, d1, d0 := encodeAcc(6.742071875)
a := decode2Acc(d2, d1, d0)
fmt.Println(a)
}
const G = 9.80665
func encodeAcc(a float32) (d2, d1, d0 byte) {
d2 = 0
d1 = 0
d0 = 0
x := a / G
v := uint32(0)
for i := 17; i >= 0; i-- {
t := float32(1.0 / math.Pow(2, float64(17-i)))
if t > x {
continue
} else {
v |= 1 << i
x -= t
}
}
fmt.Printf("%b, %b\n", v, v<<6)
v <<= 6
d0 = byte(v)
d1 = byte(v >> 8)
d2 = byte(v >> 16)
fmt.Printf("%b, %b, %b\n", d2, d1, d0)
return
}
func decode2Acc(d2, d1, d0 byte) float32 {
v := uint32(d2)<<10 | uint32(d1)<<2 | uint32(d0>>6)
fmt.Printf("%b\n", v)
x := float32(0)
for i := 17; i >= 0; i-- {
if v&(1<<i) != 0 {
t := float32(1.0 / math.Pow(2, float64(17-i)))
x += t
}
}
fmt.Println(x)
return x * G
}

6
go.mod
View File

@ -46,10 +46,16 @@ require (
github.com/jinzhu/now v1.1.5 // indirect
github.com/josharian/intern v1.0.0 // indirect
github.com/mailru/easyjson v0.7.7 // indirect
github.com/mohae/deepcopy v0.0.0-20170929034955-c48cc78d4826 // indirect
github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 // indirect
github.com/richardlehane/mscfb v1.0.4 // indirect
github.com/richardlehane/msoleps v1.0.3 // indirect
github.com/sagikazarmark/locafero v0.4.0 // indirect
github.com/simonvetter/modbus v1.6.0 // indirect
github.com/sourcegraph/conc v0.3.0 // indirect
github.com/xuri/efp v0.0.0-20231025114914-d1ff6096ae53 // indirect
github.com/xuri/excelize/v2 v2.8.1 // indirect
github.com/xuri/nfp v0.0.0-20230919160717-d98342af3f05 // indirect
github.com/yohamta/donburi v1.3.9 // indirect
go.uber.org/atomic v1.9.0 // indirect
go.uber.org/multierr v1.9.0 // indirect

13
go.sum
View File

@ -153,6 +153,8 @@ github.com/modern-go/concurrent v0.0.0-20180306012644-bacd9c7ef1dd h1:TRLaZ9cD/w
github.com/modern-go/concurrent v0.0.0-20180306012644-bacd9c7ef1dd/go.mod h1:6dJC0mAP4ikYIbvyc7fijjWJddQyLn8Ig3JB5CqoB9Q=
github.com/modern-go/reflect2 v1.0.2 h1:xBagoLtFs94CBntxluKeaWgTMpvLxC4ur3nMaC9Gz0M=
github.com/modern-go/reflect2 v1.0.2/go.mod h1:yWuevngMOJpCy52FWWMvUC8ws7m/LJsjYzDa0/r8luk=
github.com/mohae/deepcopy v0.0.0-20170929034955-c48cc78d4826 h1:RWengNIwukTxcDr9M+97sNutRR1RKhG96O6jWumTTnw=
github.com/mohae/deepcopy v0.0.0-20170929034955-c48cc78d4826/go.mod h1:TaXosZuwdSHYgviHp1DAtfrULt5eUgsSMsZf+YrPgl8=
github.com/natefinch/lumberjack v2.0.0+incompatible h1:4QJd3OLAMgj7ph+yZTuX13Ld4UpgHp07nNdFX7mqFfM=
github.com/natefinch/lumberjack v2.0.0+incompatible/go.mod h1:Wi9p2TTF5DG5oU+6YfsmYQpsTIOm0B1VNzQg9Mw6nPk=
github.com/niemeyer/pretty v0.0.0-20200227124842-a10e7caefd8e/go.mod h1:zD1mROLANZcx1PVRCS0qkT7pwLkGfwJo4zjcN/Tysno=
@ -164,6 +166,11 @@ github.com/pkg/diff v0.0.0-20210226163009-20ebb0f2a09e/go.mod h1:pJLUxLENpZxwdsK
github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4=
github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2 h1:Jamvg5psRIccs7FGNTlIRMkT8wgtp5eCXdBlqhYGL6U=
github.com/pmezard/go-difflib v1.0.1-0.20181226105442-5d4384ee4fb2/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4=
github.com/richardlehane/mscfb v1.0.4 h1:WULscsljNPConisD5hR0+OyZjwK46Pfyr6mPu5ZawpM=
github.com/richardlehane/mscfb v1.0.4/go.mod h1:YzVpcZg9czvAuhk9T+a3avCpcFPMUWm7gK3DypaEsUk=
github.com/richardlehane/msoleps v1.0.1/go.mod h1:BWev5JBpU9Ko2WAgmZEuiz4/u3ZYTKbjLycmwiWUfWg=
github.com/richardlehane/msoleps v1.0.3 h1:aznSZzrwYRl3rLKRT3gUk9am7T/mLNSnJINvN0AQoVM=
github.com/richardlehane/msoleps v1.0.3/go.mod h1:BWev5JBpU9Ko2WAgmZEuiz4/u3ZYTKbjLycmwiWUfWg=
github.com/rogpeppe/go-internal v1.6.1/go.mod h1:xXDCJY+GAPziupqXw64V24skbSoqbTEfhy4qGm1nDQc=
github.com/rogpeppe/go-internal v1.8.0/go.mod h1:WmiCO8CzOY8rg0OYDC4/i/2WRWAB6poM+XZ2dLUbcbE=
github.com/rogpeppe/go-internal v1.9.0 h1:73kH8U+JUqXU8lRuOHeVHaa/SZPifC7BkcraZVejAe8=
@ -220,6 +227,12 @@ github.com/ugorji/go v1.2.7/go.mod h1:nF9osbDWLy6bDVv/Rtoh6QgnvNDpmCalQV5urGCCS6
github.com/ugorji/go/codec v1.2.7/go.mod h1:WGN1fab3R1fzQlVQTkfxVtIBhWDRqOviHU95kRgeqEY=
github.com/ugorji/go/codec v1.2.11 h1:BMaWp1Bb6fHwEtbplGBGJ498wD+LKlNSl25MjdZY4dU=
github.com/ugorji/go/codec v1.2.11/go.mod h1:UNopzCgEMSXjBc6AOMqYvWC1ktqTAfzJZUZgYf6w6lg=
github.com/xuri/efp v0.0.0-20231025114914-d1ff6096ae53 h1:Chd9DkqERQQuHpXjR/HSV1jLZA6uaoiwwH3vSuF3IW0=
github.com/xuri/efp v0.0.0-20231025114914-d1ff6096ae53/go.mod h1:ybY/Jr0T0GTCnYjKqmdwxyxn2BQf2RcQIIvex5QldPI=
github.com/xuri/excelize/v2 v2.8.1 h1:pZLMEwK8ep+CLIUWpWmvW8IWE/yxqG0I1xcN6cVMGuQ=
github.com/xuri/excelize/v2 v2.8.1/go.mod h1:oli1E4C3Pa5RXg1TBXn4ENCXDV5JUMlBluUhG7c+CEE=
github.com/xuri/nfp v0.0.0-20230919160717-d98342af3f05 h1:qhbILQo1K3mphbwKh1vNm4oGezE1eF9fQWmNiIpSfI4=
github.com/xuri/nfp v0.0.0-20230919160717-d98342af3f05/go.mod h1:WwHg+CVyzlv/TX9xqBFXEZAuxOPxn2k1GNHwG41IIUQ=
github.com/yohamta/donburi v1.3.9 h1:sYAPaelSnxmoTGjgH9ZlYt4pUKrnwvAv4YGXxLZCK6E=
github.com/yohamta/donburi v1.3.9/go.mod h1:5QkyraUjkzbMVTD2b8jaPFy1Uwjm/zdFN1c1lZGaezg=
github.com/yousifnimah/Cryptx v1.0.1 h1:oZbB6CjvqOzkmoghsncb3/4sV2As8nqOJCjZPd08Xdw=

1
go.work.sum
View File

@ -266,6 +266,7 @@ go.uber.org/zap v1.21.0 h1:WefMeulhovoZ2sYXz7st6K0sLj7bBhpiFaud4r4zST8=
go.uber.org/zap v1.21.0/go.mod h1:wjWOCqI0f2ZZrJF/UufIOkiC8ii6tm1iqIsLo76RfJw=
golang.org/x/image v0.1.0 h1:r8Oj8ZA2Xy12/b5KZYj3tuv7NG/fBz3TwQVvpJ9l8Rk=
golang.org/x/image v0.1.0/go.mod h1:iyPr49SD/G/TBxYVB/9RRtGUT5eNbo2u4NamWeQcD5c=
golang.org/x/image v0.14.0/go.mod h1:HUYqC05R2ZcZ3ejNQsIHQDQiwWM4JBqmm6MKANTp4LE=
golang.org/x/mobile v0.0.0-20221012134814-c746ac228303 h1:K4fp1rDuJBz0FCPAWzIJwnzwNEM7S6yobdZzMrZ/Zws=
golang.org/x/mobile v0.0.0-20221012134814-c746ac228303/go.mod h1:M32cGdzp91A8Ex9qQtyZinr19EYxzkFqDjW2oyHzTDQ=
golang.org/x/net v0.10.0/go.mod h1:0qNGK6F8kojg2nk9dLZ2mShWaEBan6FAoqfSigmmuDg=

2
rts-sim-module

@ -1 +1 @@
Subproject commit 86c02883c4993bf829599c7516862009077d586b
Subproject commit efa7d1655ed781512c8b0ce56685954fa3cc9903

184
third_party/balise/codec.go vendored
View File

@ -1,6 +1,10 @@
package balise
import "fmt"
import (
"fmt"
"log/slog"
"strconv"
)
// 应答器数据编解码器
type Codec interface {
@ -8,22 +12,172 @@ type Codec interface {
const (
Bytes1023 = 128
Bytes341 = 43
)
// 解码应答器数据,1023/341位解码
// bys - 128/43字节数据
// return - 830/210位数据
func Decode(bys []byte) ([]int, error) {
size := len(bys)
func Decode(byte128 []byte) ([]byte, error) {
if len(byte128) != Bytes1023 {
return nil, buildError("应答器报文长度错误, 期望长度为128字节")
}
bits1023 := convertTo1023Bits(byte128)
// 检查控制位
err := checkCb(bits1023)
if err != nil {
return nil, err
}
// 检查913位数据，并将913位数据转换为830位数据
data913 := getRange(bits1023, 1022, 110)
scrambled830, err := convert913To830(data913)
if err != nil {
return nil, err
}
// 根据加扰位计算得到加扰器初始值S
S := calculateS(getRange(bits1023, 106, 95))
// 解扰
descrambled830 := descrambling(scrambled830, S)
// 还原第一个10位值
reverted830 := revertFirst10Bits(descrambled830)
// 转换为左边为最高有效位（MSB）的字节数组
byte104 := toBytes(reverted830)
return byte104, nil
}
if size == Bytes1023 {
// 1023应答器解码
return nil, nil
} else if size == Bytes341 {
// 341应答器解码
return nil, nil
} else {
return nil, fmt.Errorf("不支持的应答器类型")
// 解码应答器数据,1023/341位解码
// msg - 128字节数据
// return - 830/210位数据
func DecodeByteString(msg string) ([]byte, error) {
length := len(msg)
if length != 256 {
panic("invalid length")
}
// 字节转换为字节数组
slog.Debug("待解码的1023应答器报文", "msg", msg)
bytes := ConvertByteStringToBytes(msg)
return Decode(bytes)
}
// 转换字节（16进制）字符串为字节数组
func ConvertByteStringToBytes(msg string) []byte {
length := len(msg)
bytes := make([]byte, length/2)
for i := 0; i < length; i += 2 {
v, err := strconv.ParseUint(msg[i:i+2], 16, 8)
if err != nil {
panic(err)
}
bytes[i/2] = byte(v)
// slog.Info("i", "byteidx", i/2, "byte", fmt.Sprintf("%02x", v))
}
return bytes
}
// 将830位补末尾补1到832位，然后转换为左边为最高有效位（MSB）的字节数组
func toBytes(reverted830 []byte) []byte {
reverted830 = append(reverted830, 1, 1)
byte104 := make([]byte, 104)
for i := 0; i < 104; i++ {
byte104[i] = byte(ToValLeftMsb(reverted830[i*8 : i*8+8]))
}
return byte104
}
func buildError(msg string) error {
return fmt.Errorf("应答器1023解码错误：%s", msg)
}
// 检查控制位
func checkCb(bits1023 []byte) error {
cb := getRange(bits1023, 109, 107)
for i, v := range cb {
n := 109 - i
name := "b" + strconv.Itoa(109-i)
slog.Info("cb", name, v)
if n == 109 && v == 1 {
return buildError("控制位cb错误，b109应该为0，实际为1")
} else if n == 108 && v == 1 {
return buildError("控制位cb错误，b108应该为0，实际为1")
} else if n == 107 && v == 0 {
return buildError("控制位cb错误，b107应该为1，实际为0")
}
}
return nil
}
func getRange(bits []byte, start, end int) []byte {
if start < 0 || end < 0 || start < end || start > 1022 {
panic("invalid range")
}
return bits[1022-start : 1022-(end-1)]
}
// 转换128字节数据为1023位数据
func convertTo1023Bits(byte128 []byte) []byte {
if len(byte128) != 128 {
panic("invalid length")
}
// 字节转换为bit数组
bits := make([]byte, 1024)
for i, bt := range byte128 {
for j := 0; j < 8; j++ {
move := 7 - j
idx := i*8 + j
bits[idx] = (bt >> move) & 1
}
}
bits1023 := bits[0:1023]
return bits1023
}
// 由加扰位计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
func calculateS(sb []byte) uint32 {
// 由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
if len(sb) != 12 {
panic("invalid length")
}
B := ToValLeftMsb(sb)
const A uint64 = 2801775573
S := uint32((A * uint64(B)) % (1 << 32))
slog.Info("由12位加扰位计算得到整数S", "B", B, "S", S, "Sb", fmt.Sprintf("%032b", S))
return S
}
// 以11位为一组比较两个913位的报文
func compare913(b913 []byte, compare []byte) {
if len(b913) != 913 {
panic("invalid length")
}
for i := 0; i < 913; i += 11 {
for j := 0; j < 11; j++ {
print(b913[i+j])
}
println()
for j := 0; j < 11; j++ {
print(compare[i+j])
}
println()
println()
}
}
// 比较830位数据
func compare830(b830 []byte, compare830 []byte) {
if len(b830) != 830 {
panic("invalid length")
}
for i := 0; i < 83; i++ {
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b830[i*10+j])
}
println()
for j := 0; j < 10; j++ {
fmt.Printf("%01b", compare830[i*10+j])
}
println()
println()
}
for i := 0; i < 830; i++ {
if b830[i] != compare830[i] {
slog.Info("error", "index", i, "b830", b830[i], "compare", compare830[i])
panic("830 bit compare error")
}
}
}

113
third_party/balise/conv.go vendored
View File

@ -113,14 +113,83 @@ var ConvWords = []uint16{
var convWordMap = make(map[uint16]int, 1024)
func init() {
if len(ConvWords) != 1024 {
panic(fmt.Errorf("ConvWords长度不是1024, len=%d", len(ConvWords)))
}
// 检查，前512个字的累加和为267528，所有1024个字的累加和为1048064
sum1 := 0
sum2 := 0
for i := 0; i < 1024; i++ {
if i < 512 {
sum1 += int(ConvWords[i])
}
sum2 += int(ConvWords[i])
}
if sum1 != 267528 {
panic(fmt.Errorf("前512个字的累加和不是267528, sum1=%d", sum1))
}
if sum2 != 1048064 {
panic(fmt.Errorf("所有1024个字的累加和不是1048064, sum2=%d", sum2))
}
// 检查，后一个字比前一个字大
for i := 1; i < 1024; i++ {
if ConvWords[i] <= ConvWords[i-1] {
panic(fmt.Errorf("第%d个字比第%d个字小, %04o <= %04o", i, i-1, ConvWords[i], ConvWords[i-1]))
}
}
for i, v := range ConvWords {
convWordMap[v] = i
fmt.Printf("%04o: %d\n", v, i)
// slog.Info("构建10位到11位转换置换字", "i", i, "v", v)
}
// 检查：翻转有效字的所有位能形成另一个有效字
for _, v := range ConvWords {
rv := revertBits(v)
_, ok := convWordMap[rv]
if !ok {
panic(fmt.Errorf("构建10位到11位转换置换字失败, v=%04o, rv=%04o", v, rv))
}
}
}
// 翻转11位bit数组
func revertBits(word uint16) uint16 {
bits11 := ToBitsLeftMsb(int(word), 11)
revert := make([]byte, 11)
for i := 0; i < 11; i++ {
if bits11[i] == 1 {
revert[i] = 0
} else {
revert[i] = 1
}
}
rw := ToValLeftMsb(revert)
// slog.Info("反转11位bit数组", "word", fmt.Sprintf("%04o", word), "bits11", bits11, "revert", revert, "revertWord", fmt.Sprintf("%04o", rw))
return rw
}
// bit数组转换为数字,左边为最高有效位
// v - 0/1数组,数组的每个值都只能是0或1
func ToVal(v []int) uint16 {
func ToValLeftMsb(v []byte) uint16 {
if len(v) > 15 {
panic(fmt.Errorf("不支持15位以上"))
}
val := uint16(0)
l := len(v)
elems := make([]string, l)
for i := 0; i < l; i++ {
elems[i] = fmt.Sprintf("%d", v[i])
if v[i] == 1 {
val += (1 << (l - i - 1))
}
}
// slog.Info("ToValLeftMsb", "len", l, "v", strings.Join(elems, ""), "val", fmt.Sprintf("%04o", val))
return val
}
// bit数组转换为数字,右边为最高有效位
// v - 0/1数组,数组的每个值都只能是0或1
func ToValRightMsb(v []byte) uint16 {
if len(v) > 15 {
panic(fmt.Errorf("不支持15位以上"))
}
@ -128,15 +197,15 @@ func ToVal(v []int) uint16 {
l := len(v)
for i := 0; i < l; i++ {
if v[i] == 1 {
val += (1 << (l - i - 1))
val += (1 << i)
}
}
return val
}
// 数字转换为bit数组,左边为最高有效位
func ToBits(val int, count int) []int {
bs := make([]int, count)
func ToBitsLeftMsb(val int, count int) []byte {
bs := make([]byte, count)
for i := 0; i < count; i++ {
tmp := 1 << (count - 1 - i)
if (val & (tmp)) == (tmp) {
@ -148,26 +217,42 @@ func ToBits(val int, count int) []int {
return bs
}
// 11位字转换回10位字
func From11(b11 []int) ([]int, error) {
v11 := ToVal(b11)
v10, ok := convWordMap[uint16(v11)]
if ok {
return ToBits(v10, 10), nil
// 数字转换为bit数组,右边为最高有效位
func ToBitsRightMsb(val int, count int) []byte {
bs := make([]byte, count)
for i := 0; i < count; i++ {
tmp := 1 << i
if (val & (tmp)) == (tmp) {
bs[i] = 1
} else {
return nil, fmt.Errorf("错误的11位字")
bs[i] = 0
}
}
return bs
}
// 11位字转换回10位字
func From11(b11 []byte) ([]byte, error) {
v11 := ToValLeftMsb(b11)
v10, ok := convWordMap[v11]
// slog.Info("11位字转换回10位字", "v11", fmt.Sprintf("%04o", v11), "v11b", fmt.Sprintf("%011b", v11), "v10", v10, "ok", ok, "v10bits", fmt.Sprintf("%010b", v10), "to10Bits", ToBitsLeftMsb(v10, 10))
if ok {
return ToBitsLeftMsb(v10, 10), nil
} else {
return nil, fmt.Errorf("错误的11位字，word11=%04o", v11)
}
}
// 10位字转换为11位字
func To11(b10 []int) []int {
func To11(b10 []byte) []byte {
if len(b10) != 10 {
panic(fmt.Errorf("应答器编码10位字转换为11位字参数异常: 位数不是10, len=%d", len(b10)))
}
v10 := ToVal(b10)
v10 := ToValLeftMsb(b10)
if v10 > 1023 {
panic(fmt.Errorf("应答器编码10位字转换为11位字参数异常: 10位字转为整数不能大于1023, v10=%d", v10))
}
v11 := ConvWords[v10]
return ToBits(int(v11), 11)
// slog.Info("10位字转换为11位字", "v10", v10, "v10b", fmt.Sprintf("%010b", v10), "v11", fmt.Sprintf("%04o", v11), "v11bits", fmt.Sprintf("%011b", v11), "to11Bits", ToBitsLeftMsb(int(v11), 11))
return ToBitsLeftMsb(int(v11), 11)
}

2
third_party/balise/conv_test.go vendored
View File

@ -39,6 +39,6 @@ func TestConvList(t *testing.T) {
}
func TestTo11(t *testing.T) {
b10 := []int{0, 0, 0, 0, 0, 0, 0, 0, 0, 1}
b10 := []byte{0, 0, 0, 0, 0, 0, 0, 0, 0, 1}
balise.To11(b10)
}

88
third_party/balise/decode.go vendored Normal file
View File

@ -0,0 +1,88 @@
package balise
import (
"fmt"
"log/slog"
)
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组，除了第一个10位值，其余值求和，然后循环2的10次方次与其他值求和结果相加后模2的10次方，若结果和第一个10位值相同，则结束，此值即为原始的第一个10位值，将此值替换为第一个10位二进制数组，依然是左边为MSB
func revertFirst10Bits(b []byte) []byte {
if len(b) != 830 {
panic("invalid length")
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组
w10s := make([]uint16, 83)
for i := 0; i < 83; i++ {
w10s[i] = uint16(ToValLeftMsb(b[i*10 : i*10+10]))
// 打印输出
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b[i*10+j])
}
print(" ")
if i != 0 && i%10 == 9 {
println()
}
}
println()
// 将除了第一个10位字整数求和
sum := uint64(0)
for i := 1; i < 83; i++ {
sum += uint64(w10s[i])
}
// 循环2的10次方次与其他值求和结果相加后模2的10次方
for i := 0; i < 1024; i++ {
test := sum + uint64(i)
if test%1024 == uint64(w10s[0]) {
w10s[0] = uint16(i)
break
}
}
slog.Info("还原第一个10位值", "sum", sum, "bits[0]", w10s[0], "bits[0]b", fmt.Sprintf("%010b", w10s[0]))
bits := make([]byte, 830)
// 将整个10位数组转换为二进制数组，依然是MSB
u0bits := ToBitsLeftMsb(int(w10s[0]), 10)
for i := 0; i < 10; i++ {
bits[i] = u0bits[i]
}
for i := 10; i < 830; i++ {
bits[i] = b[i]
}
return bits
}
// 解扰，由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行解扰
func descrambling(dn []byte, S uint32) []byte {
if len(dn) != 830 {
panic("invalid length")
}
// const Polynomial = 0x000000AF
out := make([]byte, len(dn))
t := S // 寄存器初始值
for i := 0; i < len(dn); i++ {
msb := (t >> 31) & 1
out[i] = (dn[i] ^ byte(msb)) & 1
// fmt.Printf("i=%d, t=%032b, msb=%d, dn=%d, out=%d\n", i, t, msb, dn[i], out[i])
xor := uint32(dn[i])
t = (xor << 30) ^ (xor << 29) ^ (xor << 28) ^ (xor << 26) ^ (xor << 24) ^ t
t = (t << 1) | xor
}
return out
}
// 转换913位数据为830位数据
func convert913To830(b913 []byte) ([]byte, error) {
if len(b913) != 913 {
panic("invalid length")
}
b830 := make([]byte, 830)
for i := 0; i < 83; i++ {
b10, err := From11(b913[i*11 : i*11+11])
if err != nil {
return nil, buildError(err.Error())
}
for j := 0; j < 10; j++ {
b830[i*10+j] = b10[j]
}
}
return b830, nil
}

86
third_party/balise/encode.go vendored Normal file
View File

@ -0,0 +1,86 @@
package balise
import (
"fmt"
"log/slog"
)
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组，然后求和后模2的10次方，得到的结果覆盖第一个10位值，然后将整个10位数组转换为二进制数组，依然是左边为MSB
func replaceFirst10Bits(b []byte) []byte {
if len(b) != 830 {
panic("invalid length")
}
// 将830位的二进制数组以10位为单位组成一个左边为最高有效位（MSB）的无符号整数数组
bits := make([]uint16, 83)
for i := 0; i < 83; i++ {
bits[i] = uint16(ToValLeftMsb(b[i*10 : i*10+10]))
// 打印输出
for j := 0; j < 10; j++ {
fmt.Printf("%01b", b[i*10+j])
}
print(" ")
if i != 0 && i%10 == 9 {
println()
}
}
println()
// 将每一个10位字整数求和后模2的10次方，得到的结果覆盖第一个10位值
sum := uint64(0)
for i := 0; i < 83; i++ {
sum += uint64(bits[i])
// fmt.Printf("i=%d, v10=%d, v10b=%010b\n", i, bits[i], bits[i])
}
bits[0] = uint16(sum % 1024)
slog.Info("替换第一个10位值", "sum", sum, "bits[0]", bits[0], "bits[0]b", fmt.Sprintf("%010b", bits[0]))
rbits := make([]byte, 830)
// 将整个10位数组转换为二进制数组，依然是MSB
u0bits := ToBitsLeftMsb(int(bits[0]), 10)
for i := 0; i < 10; i++ {
rbits[i] = u0bits[i]
}
for i := 10; i < 830; i++ {
rbits[i] = b[i]
}
// compare830(b, rbits)
return rbits
}
// 由加扰计算得到的S，作为初始状态为S的32位线性反馈移位寄存器对数据进行加扰
// 1. 生成一个32位的线性反馈移位寄存器，其初始状态为S（左边为MSB）
// 2. 系数h31,h30,h29,h27,h25和h0等于1(表示连接)，所有其他系数都为0（表示不连接）
// 3. 然后电路被时钟驱动m-1次，其中m是数据位的数量，同时输入dn的每一位dn(m-1),dn(m-2),...,dn(0),便生成加扰后的码位（在第一个时钟之前读取第一个输出out(m-1)）
// 4. 生成的加扰码位是dn的每一位与S的最高位的异或值
// 5. 生成的加扰码位会根据系数进行异或反馈回S的最低位
// 几种可能性：
func scrambling(dn []byte, S uint32) []byte {
if len(dn) != 830 {
panic("invalid length")
}
// const Polynomial = 0x000000AF
out := make([]byte, len(dn))
t := S // 寄存器初始值
for i := 0; i < len(dn); i++ {
msb := (t >> 31) & 1
out[i] = (dn[i] ^ byte(msb)) & 1
// fmt.Printf("i=%d, t=%032b, msb=%d, dn=%d, out=%d\n", i, t, msb, dn[i], out[i])
xor := uint32(out[i])
t = (xor << 30) ^ (xor << 29) ^ (xor << 28) ^ (xor << 26) ^ (xor << 24) ^ t
t = (t << 1) | xor
}
return out
}
// 将830位的二进制数组先以10位为一组分别转换为11位并组合
func convert830To913(b830 []byte) []byte {
if len(b830) != 830 {
panic("invalid length")
}
b913 := make([]byte, 913)
for i := 0; i < 83; i++ {
b11 := To11(b830[i*10 : i*10+10])
for j := 0; j < 11; j++ {
b913[i*11+j] = b11[j]
}
}
return b913
}

63
third_party/can_btm/balise_btm.go vendored
View File

@ -55,33 +55,62 @@ type btmCanetClient struct {
baliseDetector *BaliseDetector
}
func (s *btmCanetClient) GetState() state_proto.BTMState {
func (s *btmCanetClient) FindNotSendState() (state_proto.BTMState, bool) {
s.baliseDetector.eqLock.Lock()
defer s.baliseDetector.eqLock.Unlock()
detector := s.baliseDetector
for _, info := range detector.eq {
if info != nil && !info.IsSend {
info.IsSend = true
return s.getStateFromDeta(info, uint32(detector.baliseCounter), uint32(detector.messageCounter), detector.aboveBalise), true
}
}
return state_proto.BTMState{}, false
}
/*
func (s *btmCanetClient) GetAllData() {
detector := s.baliseDetector
for _, info := range detector.eq {
slog.Info("获取应答器信息 id:%v,是否发送:%v", info.BaliseId)
}
}
*/
func (s *btmCanetClient) getStateFromDeta(info *BtmAntennaScanningBaliseInfo, bc, mc uint32, aboveBalise bool) state_proto.BTMState {
var telegram string
var tel128 string
info := detector.eq[len(detector.eq)-1]
var dis int64
var baliseId string
if /*detector.aboveBalise &&*/ info != nil && len(info.telegram) != 0 {
telegram = fmt.Sprintf("%X", info.telegram)
tel128 = fmt.Sprintf("%X", info.telegram128)
dis = info.Distance
baliseId = info.BaliseId
} else {
telegram = strings.Repeat("00", balise_const.UserTelegramByteLen)
tel128 = strings.Repeat("00", balise_const.TelegramByteLen)
}
var dis int64
if info != nil {
dis = info.Distance
}
return state_proto.BTMState{
BaliseId: baliseId,
DataSerialNumber: uint32(s.dsn),
BaliseCount: uint32(detector.baliseCounter),
MessageCounter: uint32(detector.messageCounter),
BaliseCount: bc,
MessageCounter: mc,
Telegram: telegram,
Telegram128: tel128,
Distance: dis,
AboveBalise: detector.aboveBalise,
AboveBalise: aboveBalise,
}
}
func (s *btmCanetClient) GetState() state_proto.BTMState {
detector := s.baliseDetector
info := detector.eq[len(detector.eq)-1]
return s.getStateFromDeta(info, uint32(detector.baliseCounter), uint32(detector.messageCounter), detector.aboveBalise)
}
type BtmClock struct {
BtmTk uint32 //与ATP系统同步的时间ms
@ -100,6 +129,8 @@ type BtmCanetClient interface {
HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state_proto.VobcBtmState, h *TrainHeadPositionInfo)
//获取BTM显示状态 + btm最新的状态
GetState() state_proto.BTMState
//GetAllData()
FindNotSendState() (state_proto.BTMState, bool)
}
var (
@ -121,7 +152,16 @@ func (s *btmCanetClient) HandleTrainHeadPositionInfo(w ecs.World, vobcBtm *state
//slog.Debug(h.String())
wd := entity.GetWorldData(w)
repo := wd.Repo
s.baliseDetector.detect(wd, repo, h, vobcBtm)
//s.baliseDetector.detect(wd, repo, h, vobcBtm)
h2 := &TrainHeadPositionInfo{
TrainId: h.TrainId,
Up: h.Up,
Link: h.OldLink,
LinkOffset: h.OldLinkOffset,
Speed: h.Speed,
Acceleration: h.Acceleration}
s.baliseDetector.detect2(wd, repo, h, h2, vobcBtm)
}
func (s *btmCanetClient) Start(bcm BtmCanetManager) {
s.bcm = bcm
@ -154,6 +194,9 @@ func (s *btmCanetClient) Stop() {
s.udpClient.Close()
s.udpClient = nil
}
for d := range s.baliseDetector.eq {
s.baliseDetector.eq[d] = nil
}
}
func (s *btmCanetClient) handleCanetFrames(cfs []byte) {
defer func() {

137
third_party/can_btm/balise_detection.go vendored
View File

@ -2,12 +2,14 @@ package can_btm
import (
"fmt"
uuid2 "github.com/google/uuid"
"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"
"joylink.club/rtsssimulation/repository/model/proto"
"log/slog"
"math"
"sort"
"sync"
@ -28,6 +30,7 @@ type BtmAntennaRunningInfo struct {
}
const (
//BtmAntennaOffsetHead = int64(1000) //车载BTM天线距车头端点的距离，mm
BtmAntennaOffsetHead = int64(1000) //车载BTM天线距车头端点的距离，mm
)
@ -46,6 +49,7 @@ type BtmAntennaScanningBaliseInfo struct {
telegram128 []byte //应答器报文
Distance int64 //BTM天线中心到应答器的距离，mm
BaliseType proto.Transponder_Type //应答器类型
IsSend bool
}
// BaliseDetector 车载BTM天线，应答器探测器
@ -76,6 +80,85 @@ func (t *BaliseDetector) tryRebind(th *TrainHeadPositionInfo) {
}
}
func (t *BaliseDetector) detect2(wd *component.WorldData, repo *repository.Repository, th, th2 *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
t.tryRebind(th)
//BTM天线中心点运行信息
curAntennaRi := t.createBtmAntennaRunningInfo(wd, repo, th) //目前车头
curAntennaRi2 := t.createBtmAntennaRunningInfo(wd, repo, th2) //上次车头
var startBalises []*repository.Transponder
var endBalises []*repository.Transponder
startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi2.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)
//endBalises = t.searchBalisesFromBetweenLinkPosition(repo, !th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset)
/* if th.Up {
startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi2.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)
endBalises = t.searchBalisesFromBetweenLinkPosition(repo, !th.Up, curAntennaRi.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset)
} else {
startBalises = t.searchBalisesFromBetweenLinkPosition(repo, th.Up, curAntennaRi2.LinkId, curAntennaRi.LinkOffset, curAntennaRi2.LinkOffset)
endBalises = t.searchBalisesFromBetweenLinkPosition(repo, !th.Up, curAntennaRi.LinkId, curAntennaRi2.LinkOffset, curAntennaRi.LinkOffset)
}*/
balises := make([]*repository.Transponder, 0)
for _, balise := range startBalises {
find := false
for _, transponder := range balises {
if transponder.Id() == balise.Id() {
find = true
break
}
}
if !find {
balises = append(balises, balise)
//slog.Info(fmt.Sprintf("start baliseId:%v,Distance:%v,up:%v", balise.Id(), balise.LinkPosition(), curAntennaRi.Up))
}
}
for _, balise := range endBalises {
find := false
for _, transponder := range balises {
if transponder.Id() == balise.Id() {
find = true
break
}
}
if !find {
//slog.Info(fmt.Sprintf("end baliseId:%v,Distance:%v,up:%v", balise.Id(), balise.LinkPosition(), curAntennaRi.Up))
balises = append(balises, balise)
}
}
if len(balises) > 0 {
balise := balises[0]
baliseInfo := &BtmAntennaScanningBaliseInfo{BaliseId: balise.Id(), BaliseType: balise.BaliseType()}
uuid := uuid2.NewString()
for _, transponder := range balises {
slog.Info(fmt.Sprintf("uid :%v,baliseId:%v,Distance:%v,up:%v", uuid, transponder.Id(), baliseInfo.Distance, curAntennaRi.Up))
}
telegram, utel := t.rcvTelegram(wd, balise.Id())
if t.addExpectedBalise(baliseInfo) {
t.baliseCounterAdd1() //应答器计数器
if len(telegram) > 0 {
baliseInfo.telegram = utel
baliseInfo.telegram128 = telegram
t.baliseMessageCounterAdd1() //报文计数器
}
}
t.aboveBalise = true
} 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())
}*/
}
func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Repository, th *TrainHeadPositionInfo, vobcBtm *state_proto.VobcBtmState) {
t.tryRebind(th)
//if !t.powerAmplifierSwitch { //天线功率放大器未开启，不进行探测
@ -88,7 +171,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 {
@ -101,7 +186,6 @@ func (t *BaliseDetector) detect(wd *component.WorldData, repo *repository.Reposi
curExpect.telegram = utel
curExpect.telegram128 = telegram
t.baliseMessageCounterAdd1() //报文计数器
}
}
//BTM天线即将经过应答器
@ -165,10 +249,17 @@ func (t *BaliseDetector) addExpectedBalise(curExpect *BtmAntennaScanningBaliseIn
// }
//}
//检查是否已经记录过
eq := t.eq[len(t.eq)-1]
if eq != nil && eq.BaliseId == curExpect.BaliseId {
for _, tt := range t.eq {
if tt != nil && tt.BaliseId == curExpect.BaliseId {
return false
}
}
/* eq := t.eq[len(t.eq)-1]
if eq != nil && eq.BaliseId == curExpect.BaliseId {
return false
}*/
//左移
for i := 1; i < len(t.eq); i++ {
t.eq[i-1] = t.eq[i]
@ -210,7 +301,9 @@ func (t *BaliseDetector) timeScanNearestBalise(curTime time.Time, wd *component.
curAc := float64(ba.Acceleration)
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, BaliseType: expectedBalise.BaliseType}
}
}
@ -284,6 +377,43 @@ func (t *BaliseDetector) findBaliseWillScanByBtmAntenna(wd *component.WorldData,
return nil
}
func (t *BaliseDetector) searchBalisesFromBetweenLinkPosition(repo *repository.Repository, up bool, linkId string, fromOffset int64, toOffset int64) []*repository.Transponder {
rs := repo.ResponderListByLink(linkId)
balises := make([]*repository.Transponder, 0)
if up {
sort.SliceStable(rs, func(i, j int) bool {
return rs[i].LinkPosition().Offset() < rs[j].LinkPosition().Offset()
})
for _, r := range rs {
if r.LinkPosition().Offset() >= fromOffset && r.LinkPosition().Offset() <= toOffset {
//slog.Info(fmt.Sprintf("up id:%v,offset:%v,from:%v,to:%v", r.Id(), r.LinkPosition().Offset(), fromOffset, toOffset))
balises = append(balises, r)
}
}
} else {
sort.SliceStable(rs, func(i, j int) bool {
return rs[j].LinkPosition().Offset() < rs[i].LinkPosition().Offset()
})
//cha := int64(math.Abs(float64(toOffset - fromOffset)))
for _, r := range rs {
/*if r.LinkPosition().Offset() <= fromOffset && r.LinkPosition().Offset() >= toOffset {
balises = append(balises, r)
}*/
if r.LinkPosition().Offset() <= toOffset {
cha := int64(math.Abs(float64(toOffset - fromOffset)))
cha2 := int64(math.Abs(float64(toOffset - r.LinkPosition().Offset())))
if cha2 <= cha {
balises = append(balises, r)
}
}
}
}
return balises
}
// up-在轨道上的搜索方向
func (t *BaliseDetector) searchBalisesFromLinkPosition(repo *repository.Repository, linkId string, up bool, fromOffset int64) []*repository.Transponder {
rs := repo.ResponderListByLink(linkId)
@ -302,6 +432,7 @@ func (t *BaliseDetector) searchBalisesFromLinkPosition(repo *repository.Reposito
return rs[j].LinkPosition().Offset() < rs[i].LinkPosition().Offset()
})
for i, r := range rs {
//slog.Info(fmt.Sprintf("id:%v,offset:%v,from:%v", r.Id(), r.LinkPosition().Offset(), fromOffset))
if r.LinkPosition().Offset() <= fromOffset {
return rs[i:]
}

4
third_party/example/main.go vendored
View File

@ -46,13 +46,13 @@ func main() {
// fmt.Println()
// }
// }
v10 := balise.ToVal([]int{
v10 := balise.ToValLeftMsb([]byte{
1, 1, 1,
1, 1, 1, 1,
1, 1, 1, 1,
1, 1, 1, 1})
fmt.Println(v10)
bs := balise.ToBits(1982, 11)
bs := balise.ToBitsLeftMsb(1982, 11)
fmt.Println(bs)
// fmt.Printf("%o\n", balise.ConvWords[511])
}

10
third_party/message/dynamics.go vendored
View File

@ -78,7 +78,7 @@ type DynamicsTrainInfo struct {
//此次计算所使用的半实物消息的生命信号
VobcLifeSignal uint16
//位移（mm）
Displacement uint16
Displacement float32
TrainActToMax bool
TrainActToMin bool
UpdateTime int64
@ -119,7 +119,13 @@ func (t *DynamicsTrainInfo) Decode(buf []byte) error {
t.TailRadarSpeed = math.Float32frombits(binary.BigEndian.Uint32(buf[56:60]))
t.Acceleration = math.Float32frombits(binary.BigEndian.Uint32(buf[60:64]))
t.VobcLifeSignal = binary.BigEndian.Uint16(buf[64:66])
t.Displacement = binary.BigEndian.Uint16(buf[66:68])
//tt := binary.BigEndian.Uint16(buf[66:68])
//t.Displacement = float32(tt)
dirBin := binary.BigEndian.Uint32(buf[66:70])
t.Displacement = math.Float32frombits(dirBin)
t.UpdateTime = time.Now().UnixMilli()
return nil
}

2
third_party/message/radar.go vendored
View File

@ -30,7 +30,7 @@ type RadarInfo struct {
Tail byte
}
func NewRadarSender(speed float32, forward bool, displacement uint16) *RadarInfo {
func NewRadarSender(speed float32, forward bool, displacement float32) *RadarInfo {
state := &RadarState{SwModel1: 1, SwModel0: 1, calculateBit: 1, signalQualityBit: 1, BlackoutBit: 0, DirectionState: 1, Direction: IsTrue(forward)}
ri := &RadarInfo{State: state}
ri.RealSpeed = uint16(math.Abs(float64(speed*3.6) / fixed_speed))

41
third_party/message/train_pc_sim_message.go vendored
View File

@ -121,35 +121,36 @@ func AtpLowPowerByte(d byte) bool {
// 列车速度位置报告
type TrainSpeedPlaceReportMsg struct {
PulseCount1 uint32
PulseCount2 uint32
PulseCount1 float32
PulseCount2 float32
}
func (tp *TrainSpeedPlaceReportMsg) ParsePulseCount1(s1, s2 uint32) {
tp.PulseCount1 += s1
tp.PulseCount2 += s2
}
func (tp *TrainSpeedPlaceReportMsg) Encode(runDir bool, s1, s2 uint32) []byte {
func (tp *TrainSpeedPlaceReportMsg) Encode(runDir uint16, s1, runRange uint32) []byte {
data := make([]byte, 0)
data = binary.BigEndian.AppendUint16(data, uint16(IsTrue(runDir)))
data = binary.BigEndian.AppendUint16(data, runDir)
data = binary.BigEndian.AppendUint32(data, s1)
data = binary.BigEndian.AppendUint32(data, s1)
data = binary.BigEndian.AppendUint32(data, s2)
data = binary.BigEndian.AppendUint32(data, tp.PulseCount1)
data = binary.BigEndian.AppendUint32(data, tp.PulseCount2)
now := time.Now().UTC()
// 将时间转换为毫秒
millis := now.UnixNano() / int64(time.Millisecond)
millisStr := strconv.Itoa(int(millis))
strs := []rune(millisStr)
second, _ := strconv.Atoi(string(strs[:len(strs)-3]))
mm, _ := strconv.Atoi(string(strs[len(strs)-3:]))
data = binary.BigEndian.AppendUint32(data, uint32(second))
data = binary.BigEndian.AppendUint16(data, uint16(mm))
data = binary.BigEndian.AppendUint32(data, runRange)
data = binary.BigEndian.AppendUint32(data, runRange)
sec, ms := tp.time()
data = binary.BigEndian.AppendUint32(data, sec)
data = binary.BigEndian.AppendUint16(data, ms)
return data
}
func (tp *TrainSpeedPlaceReportMsg) time() (uint32, uint16) {
now := time.Now().UnixMilli()
timeNowStr := strconv.Itoa(int(now))
timeStrs := []rune(timeNowStr)
sec, _ := strconv.Atoi(string(timeStrs[5:10]))
ms, _ := strconv.Atoi(string(timeStrs[10:]))
return uint32(sec), uint16(ms)
}
func (tp *TrainSpeedPlaceReportMsg) Decode(d []byte) {
buf := bytes.NewBuffer(d)
var runDir uint16

99
third_party/train_pc_sim/train_pc_sim.go vendored
View File

@ -37,7 +37,7 @@ type TrainPcSim interface {
SendTrainDirection(train *state_proto.TrainState, trainForward, trainBackward bool)
//发送应答器信息数据
SendBaliseData(train *state_proto.TrainState, msgType byte, data []byte)
SendBaliseData2(train *state_proto.TrainState, msgType byte, data []string)
//发布列车控制的相关事件
//PublishTrainControlEvent(train *state_proto.TrainState, events []TrainControlEvent)
@ -48,8 +48,7 @@ type TrainPcSim interface {
CreateOrRemoveTrain(train *state_proto.TrainState, isCreate bool) error
// TrainPluseCount 计算列车脉冲
TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32)
ResetPluseCount(sta *state_proto.TrainState)
//FindAllThirdPartState() []tpapi.ThirdPartyApiService
DsnAddAndReturn() byte
}
@ -74,7 +73,8 @@ type TrainPcSimManage interface {
type trainPcSimService struct {
state tpapi.ThirdPartyApiServiceState
newPcSimclientMap map[string]*TrainPcReciverData
cancleContext context.CancelFunc
cancleContextFun context.CancelFunc
context context.Context
trainPcSimManage TrainPcSimManage
configs []config.VehiclePCSimConfig
btmDsn uint8
@ -137,44 +137,69 @@ func (d *trainPcSimService) findAllThirdPartState() []tpapi.ThirdPartyApiService
return services
}
// 速度(单位mm/s)对应的脉冲数：速度*200/pi/840；
// 里程（单位mm）对应的脉冲：总里程*200/pi/840
func pluseCountSpeed(wheelDiameter int32, speedMeter float32) uint32 {
s1 := speedMeter * 1000
pluseCountData := s1 * 200 / math.Pi / float32(wheelDiameter)
pluseCountData := speedMeter * 200 / math.Pi / float32(wheelDiameter)
return uint32(pluseCountData)
}
func (d *trainPcSimService) ResetPluseCount(sta *state_proto.TrainState) {
if sd, err := d.findTrainConn(sta); err == nil {
sd.speedPlace.PulseCount1 = 0
sd.speedPlace.PulseCount2 = 0
func (d *trainPcSimService) pluseSpeed(sta *state_proto.TrainState) (uint32, float32) {
defer initLock.Unlock()
initLock.Lock()
var sum float32 = 0
pcLen := len(sta.PluseCount.PulseCount3)
if pcLen == 0 {
return 0, 0
}
for _, f := range sta.PluseCount.PulseCount3 {
sum += f
}
d.TrainPluseCountReset(sta)
speed := sum / float32(pcLen)
return pluseCountSpeed(sta.WheelDiameter, speed*1000), speed
}
func (d *trainPcSimService) TrainPluseCount(sta *state_proto.TrainState, h1, h2, t1, t2 float32) {
defer initLock.Unlock()
initLock.Lock()
select {
case <-d.context.Done():
return
default:
}
if sd, err := d.findTrainConn(sta); err == nil {
sd.speedPlace.PulseCount1 += sta.DynamicState.Displacement
sd.speedPlace.PulseCount2 = sd.speedPlace.PulseCount1
}
if sta.TrainRunUp {
if sta.TrainEndsA.SpeedSensorEnableA {
sta.PluseCount.PulseCount1 = pluseCountSpeed(sta.WheelDiameter, h1)
sta.PluseCount.PulseCount3 = append(sta.PluseCount.PulseCount3, h1)
}
if sta.TrainEndsA.SpeedSensorEnableB {
sta.PluseCount.PulseCount2 = pluseCountSpeed(sta.WheelDiameter, h2)
sta.PluseCount.PulseCount4 = append(sta.PluseCount.PulseCount3, h2)
}
} else {
if sta.TrainEndsB.SpeedSensorEnableA {
sta.PluseCount.PulseCount1 = pluseCountSpeed(sta.WheelDiameter, t1)
sta.PluseCount.PulseCount3 = append(sta.PluseCount.PulseCount3, t1)
}
if sta.TrainEndsB.SpeedSensorEnableB {
sta.PluseCount.PulseCount2 = pluseCountSpeed(sta.WheelDiameter, t2)
sta.PluseCount.PulseCount4 = append(sta.PluseCount.PulseCount3, t2)
}
}
}
func (d *trainPcSimService) TrainPluseCountReset(sta *state_proto.TrainState) {
defer initLock.Unlock()
initLock.Lock()
sta.PluseCount.PulseCount1 = 0
sta.PluseCount.PulseCount2 = 0
sta.PluseCount.PulseCount3 = make([]float32, 0)
sta.PluseCount.PulseCount4 = make([]float32, 0)
}
func (d *trainPcSimService) newCloseAllConn() {
@ -191,6 +216,7 @@ func (d *trainPcSimService) newCloseConn(clientKey string) {
rd.tcpClient = nil
rd.train = nil
rd.speedPlace = nil
rd.trainInit = false
}
}
@ -259,9 +285,9 @@ func (d *trainPcSimService) Start(pcSimManage TrainPcSimManage) {
d.configs = configs
ctx, ctxFun := context.WithCancel(context.Background())
d.cancleContext = ctxFun
d.cancleContextFun = ctxFun
d.context = ctx
d.trainPcSimManage = pcSimManage
//d.connTrainPcSim(ctx)
go d.sendTrainLocationAndSpeedTask(ctx)
}
@ -269,9 +295,9 @@ func (d *trainPcSimService) Stop() {
for _, data := range d.newPcSimclientMap {
data.updateState(tpapi.ThirdPartyState_Closed)
}
if d.cancleContext != nil {
d.cancleContext()
d.cancleContext = nil
if d.cancleContextFun != nil {
d.cancleContextFun()
d.cancleContextFun = nil
}
d.newCloseAllConn()
}
@ -303,13 +329,13 @@ func (d *trainPcSimService) CreateOrRemoveTrain(train *state_proto.TrainState, i
func (d *trainPcSimService) initTrain(rd *TrainPcReciverData, train *state_proto.TrainState, isCreate bool, trains *message.TrainPcSimBaseMessage) error {
msgs := make([]message.TrainPcSimBaseMessage, 0)
sendMsg := make([]byte, 0)
if isCreate {
rd.speedPlace = &message.TrainSpeedPlaceReportMsg{}
train.PluseCount = &state_proto.SensorSpeedPulseCount{}
rd.train = train
tcc := train.Tcc
tcc.LineInitTimeStamp12 = 0
tcc.Line12ConnErr = false
if isCreate {
tmpMsgs := d.trainPcSimManage.ObtainTrainDigitalMockData(train)
msgs = append(msgs, tmpMsgs...)
msgs = append(msgs, message.TrainPcSimBaseMessage{Data: []byte{0x00}, Type: message.RECIVE_TRAIN_DOOR_MODE}) //门模式
@ -317,7 +343,7 @@ func (d *trainPcSimService) initTrain(rd *TrainPcReciverData, train *state_proto
} else {
train.VobcState.Tc1Active = false
train.VobcState.Tc2Active = false
tcc := train.Tcc
for _, key := range tcc.DriverKey {
key.Val = false
}
@ -358,20 +384,22 @@ func (d *trainPcSimService) sendTrainLocationAndSpeedTask(ctx context.Context) {
connState = tpapi.ThirdPartyState_Broken
}
trainClient.updateState(connState)
s1, s2 := train.PluseCount.PulseCount1, train.PluseCount.PulseCount2
trainClient.speedPlace.ParsePulseCount1(s1, s2)
data := trainClient.speedPlace.Encode(train.TrainRunUp, s1, s2)
s1, _ := d.pluseSpeed(train)
runDir := uint16(2)
if train.Tcc.DirKey.Val == 1 {
runDir = 1
}
disPluse := pluseCountSpeed(train.WheelDiameter, trainClient.speedPlace.PulseCount1)
data := trainClient.speedPlace.Encode(runDir, s1, disPluse)
bm := &message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_LOCATION_INFO, Data: data}
d.TrainPluseCountReset(train)
dataCode := bm.Encode()
//slog.Info(fmt.Sprintf("发送列车速度位置,列车：%v,s1: %v,s2: %v,c2: %v,c2: %v,发送数据:%v", train.Id, s1, s2, trainClient.speedPlace.PulseCount1, trainClient.speedPlace.PulseCount2, hex.EncodeToString(dataCode)))
//slog.Info(fmt.Sprintf("发送列车速度位置,列车：%v,列车速度:%v,计数脉冲: %v,累计里程: %v ,发送数据:%v", train.Id, speed, s1, trainClient.speedPlace.PulseCount1, hex.EncodeToString(dataCode)))
err := trainClient.tcpClient.Send(dataCode)
if err != nil {
slog.Error(fmt.Sprintf("发送列车速度位置失败,列车：%v,发送数据:%v", train.Id, hex.EncodeToString(dataCode)))
}
}
}
time.Sleep(time.Millisecond * 80)
}
}
@ -489,25 +517,6 @@ func (d *trainPcSimService) SendTrainDirection(train *state_proto.TrainState, tr
}
}
func (d *trainPcSimService) SendBaliseData2(train *state_proto.TrainState, msgType byte, data []string) {
trainClient, trainDataErr := d.findTrainConn(train)
if trainDataErr != nil {
slog.Error(fmt.Sprintf("发送列车PC仿真应答器信息失败2，未找到列车连接，trainId：%v", train.Id))
return
}
for _, hexData := range data {
dd, _ := hex.DecodeString(hexData)
msg := &message.TrainPcSimBaseMessage{Type: msgType, Data: dd}
da := msg.Encode()
slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息,数据类型:0x%X,数据:%v", msgType, hex.EncodeToString(da)))
err := trainClient.tcpClient.Send(da)
if err != nil {
slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息失败,数据:%v", hex.EncodeToString(da)))
}
}
}
func (d *trainPcSimService) SendBaliseData(train *state_proto.TrainState, msgType byte, data []byte) {
trainClient, trainDataErr := d.findTrainConn(train)
if trainDataErr != nil {
@ -519,7 +528,7 @@ func (d *trainPcSimService) SendBaliseData(train *state_proto.TrainState, msgTyp
msg.Data = data
da := msg.Encode()
slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息,数据类型:0x%x，源数据长度:%v,数据:%v", msgType, len(data), hex.EncodeToString(da)))
//slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息,数据类型:0x%x，源数据长度:%v,数据:%v", msgType, len(data), hex.EncodeToString(da)))
err := trainClient.tcpClient.Send(da)
if err != nil {
slog.Info(fmt.Sprintf("发送列车PC仿真应答器信息失败,数据:%v", hex.EncodeToString(da)))

2
ts/simulation/wayside/memory/train_tcc_graphic.go
View File

@ -116,7 +116,7 @@ func initTrainVobc(trainLoad int64, trainIsUp bool) (*state_proto.TrainVobcState
}
// 初始化列车控制数据
func initTrainTcc(vs *VerifySimulation, runDir bool, breaking int32) *state_proto.TrainControlState {
func initTrainTcc(vs *VerifySimulation) *state_proto.TrainControlState {
var tccGI *data_proto.TccGraphicStorage
for _, id := range vs.MapIds {
if QueryGiType(id) == data_proto.PictureType_TrainControlCab {

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

@ -22,10 +22,19 @@ import (
)
const (
//列车总质量(吨)
DEFULAT_TRAIN_LOAD = 160
DEFAULT_BRAKE_FORCE = 19040
//通号最大制动力是-1.21
DEFAULT_BRAKE_FORCE = DEFULAT_TRAIN_LOAD * 1000 * 1.21 / 1000
//通号最大加速度是0.97
DEFAULT_TRAIN_TRACTION = DEFULAT_TRAIN_LOAD * 1000 * 0.97 / 1000 //千牛
DEFAULT_TRAIN_WORK = 2880 * 1000
DEFAULT_TRAIN_STARTED_SPEED = 0.003
)
func culTrainTractionF(sta *state_proto.TrainState) {
}
func CreateMsgTrainConfig(trainId int, trainLen int64, configTrainData dto.ConfigTrainData) *message.TrainOperationConfig {
return &message.TrainOperationConfig{TrainIndex: trainId, Length: int(trainLen),
DavisParamA: configTrainData.DavisParamA, DavisParamB: configTrainData.DavisParamB,
@ -51,7 +60,7 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
}
//向动力学发送初始化请求
trainIndex, _ := strconv.ParseUint(status.Id, 10, 16)
slog.Debug("添加列车", "trainIndex", trainIndex, "HeadDeviceId", status.HeadDeviceId, "HeadOffset", status.HeadOffset)
//slog.Debug("添加列车", "trainIndex", trainIndex, "HeadDeviceId", status.HeadDeviceId, "HeadOffset", status.HeadOffset)
// 映射link、偏移量、运行方向
var uid string
if status.DevicePort == "" {
@ -95,7 +104,7 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
createOrUpdateStateDynamicConfig(status, configTrainData, trainEndsA, trainEndsB)
tl := configTrainData.TrainLoad
if tl <= 0 {
tl = DEFULAT_TRAIN_LOAD
tl = DEFULAT_TRAIN_LOAD * 100
}
vobc, _ := initTrainVobc(int64(tl), status.TrainRunUp)
@ -103,9 +112,9 @@ func AddTrainStateNew(vs *VerifySimulation, status *state_proto.TrainState, conf
//status.TrainActiveDirection = trainActDir
status.Tcc = initTrainTcc(vs, status.TrainRunUp, DEFAULT_BRAKE_FORCE)
status.Tcc = initTrainTcc(vs)
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)
//slog.Debug("列车初始化", "trainIndex", trainIndex, "linkId", linkId, "loffset", loffset)
linkIdInt, _ := strconv.Atoi(linkId)
err := dynamics.Default().RequestAddTrain(&message.InitTrainInfo{
TrainIndex: uint16(trainIndex),
@ -311,7 +320,7 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
//pointTO 指的是是否ab，或是否到岔心
_, 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, "是否上行", true, "是否ab", pointTo, "t1Dir:", info.TrainActToMax, "t2Dir:", info.TrainActToMin)
trainHeadActUp := true
if info.TrainActToMax || info.TrainActToMin {
if info.TrainActToMin {
@ -326,12 +335,23 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
panic(sys_error.New("动力学传输数据:列车车尾位置计算出错", e2))
}
//slog.Debug("车尾位置", tailDeviceId, "偏移", tailDeviceOffset, "所在设备端", tailDevicePort)
updateTrainBtmPosition(vs, info, sta, outLinkId, outLinkOffset)
// 修改world中的列车位置
handleTrainPositionFromDynamic(vs, info, sta, outLinkId, outLinkOffset, tailLinkId, tailLinkOffset)
//修改列车激活方向
updateTrainActiveDirFromDynamic(vs, info, sta, id, port, trainHeadActUp)
if sta.OldLinkOffset == 0 {
sta.OldLinkOffset = outLinkOffset
sta.OldLink = outLinkId
}
//slog.Info(fmt.Sprintf("old:%v ,new :%v,--:%v", outLinkOffset, sta.OldLinkOffset, pointTo))
updateTrainBtmPosition(vs, info, sta, outLinkId, outLinkOffset)
//slog.Info(fmt.Sprintf("动力学，当前速度(米/秒):%v,加速度:%v", info.Speed, info.Acceleration))
if sta.OldLink != outLinkId {
sta.OldLink = outLinkId
}
if sta.OldLinkOffset != outLinkOffset {
sta.OldLinkOffset = outLinkOffset
}
sta.HeadDeviceId = vs.GetComIdByUid(id)
sta.DevicePort = port
sta.HeadOffset = offset
@ -364,7 +384,8 @@ func UpdateTrainStateByDynamics(vs *VerifySimulation, trainId string, info *mess
sta.DynamicState.TailRadarSpeed = speedParse(info.TailRadarSpeed)
sta.DynamicState.Acceleration = info.Acceleration
sta.DynamicState.Displacement = int32(info.Displacement)
sta.DynamicState.Displacement = info.Displacement
//slog.Info(fmt.Sprintf("动力学返回速度:%v,位移:%v,加速度:%v，time:%v", info.Speed, info.Displacement, info.Acceleration, time.Now().UnixMilli()))
pluseCount(sta, info.HeadSpeed1, info.HeadSpeed2, info.TailSpeed1, info.TailSpeed2)
return sta
}
@ -388,6 +409,8 @@ func updateTrainActiveDirFromDynamic(vs *VerifySimulation, info *message.Dynamic
// 根据列车位置修改列车应答器
func updateTrainBtmPosition(vs *VerifySimulation, info *message.DynamicsTrainInfo, sta *state_proto.TrainState, outLinkId string, outLinkOffset int64) {
// 更新BTM中列车位置信息
//isup := sta.TrainActiveDirection == 1
can_btm.Default().HandleTrainHeadPositionInfo(vs.World, sta.VobcBtm, &fi.TrainHeadPositionInfo{
TrainId: sta.Id,
Up: info.Up,
@ -395,12 +418,16 @@ func updateTrainBtmPosition(vs *VerifySimulation, info *message.DynamicsTrainInf
LinkOffset: outLinkOffset,
Speed: info.Speed,
Acceleration: info.Acceleration,
OldLinkOffset: sta.OldLinkOffset,
OldLink: sta.OldLink,
})
state := can_btm.Default().GetState()
//fmt.Println(state.Telegram)
if sta.BtmState == nil || sta.BtmState.BaliseId != state.BaliseId {
sta.BtmState = &state
}
}
// 根据动力学修改列车位置
func handleTrainPositionFromDynamic(vs *VerifySimulation, info *message.DynamicsTrainInfo, sta *state_proto.TrainState, outLinkId string, outLinkOffset int64, tailLinkId string, tailLinkOffset int64) {
fi.UpdateTrainPositionFromDynamics(vs.World, fi.TrainPositionInfo{
@ -459,7 +486,7 @@ func removeTrain(vs *VerifySimulation, trainId string, train *state_proto.TrainS
vobc.TractionStatus = false
vobc.BrakingStatus = true
vobc.TractionForce = 0
vobc.BrakeForce = DEFAULT_BRAKE_FORCE
vobc.BrakeForce = int64(DEFAULT_BRAKE_FORCE * 100)
}
train.Show = false

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

@ -4,6 +4,7 @@ import (
"encoding/binary"
"encoding/hex"
"fmt"
uuid2 "github.com/google/uuid"
"joylink.club/bj-rtsts-server/config"
"joylink.club/bj-rtsts-server/const/balise_const"
"joylink.club/bj-rtsts-server/dto/data_proto"
@ -15,6 +16,7 @@ import (
"joylink.club/bj-rtsts-server/third_party/message"
train_pc_sim "joylink.club/bj-rtsts-server/third_party/train_pc_sim"
"log/slog"
"math"
"strings"
"time"
)
@ -50,7 +52,7 @@ func ControlTrainUpdate(s *VerifySimulation, ct *request_proto.TrainControl) {
//此处先注释，根据现场调试情况 2024-4-16
train_pc_sim.Default().SendTrainDirection(sta, sta.VobcState.DirectionForward, sta.VobcState.DirectionBackward)
} else if ct.ControlType == request_proto.TrainControl_HANDLER {
if vobc.Tc1Active && vobc.Tc2Active {
if !vobc.Tc1Active && !vobc.Tc2Active {
panic(sys_error.New("TC1和TC2都未激活不能搬动牵引制动手柄 "))
}
oldTraction := sta.VobcState.TractionForce
@ -138,8 +140,11 @@ func controlEBBtn(vobc *state_proto.TrainVobcState, active bool, tccBtn *state_p
tccBtn.Passed = active
vobc.EmergencyBrakingStatus = true
vobc.TractionForce = 0
vobc.BrakeForce = DEFAULT_BRAKE_FORCE
return []message.TrainPcSimBaseMessage{{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.OUTER_EMERGENCY_BRAKE, 0}}, {Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.TRAIN_BRAKE_STATE, 1}}}
vobc.BrakeForce = int64(DEFAULT_BRAKE_FORCE * 100)
vobc.BrakingStatus = true
return []message.TrainPcSimBaseMessage{
{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.OUTER_EMERGENCY_BRAKE, 0}},
{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.TRAIN_BRAKE_STATE, 1}}}
}
@ -402,27 +407,31 @@ func trainControlHandle(vobc *state_proto.TrainVobcState, tcc *state_proto.Train
slog.Error("未找到对应的牵引制动手柄设备deviceId:", deviceId)
return nil
}
/*trainStart := false
if tcc.PushHandler.Val <= 0 {
trainStart = true
} else if tcc.PushHandler.Val > 0 {
}*/
jjzdBtn := tcc.Buttons[JJZD]
vobc.TractionStatus = false
vobc.TractionForce = 0
vobc.BrakingStatus = false
vobc.BrakeForce = 0
vobc.MaintainBrakeStatus = false
notBreak := byte(0)
var zeroState byte = 0
var brakeState byte = 0
if request.Val > 0 {
vobc.TractionStatus = true
vobc.TractionForce = int64(request.Val * 180)
vobc.TractionForce = int64(float32(request.Val)/100*DEFAULT_TRAIN_TRACTION) * 100
notBreak = 1
} else if request.Val < 0 {
vobc.BrakingStatus = true
vobc.BrakeForce = int64(-request.Val * 180)
vobc.BrakeForce = -int64(math.Abs(float64(request.Val))/100*DEFAULT_BRAKE_FORCE) * 100
vobc.EmergencyBrakingStatus = false
jjzdBtn.Passed = false
brakeState = 1
brakeState = 0
} else {
zeroState = 1
}
@ -433,7 +442,8 @@ func trainControlHandle(vobc *state_proto.TrainVobcState, tcc *state_proto.Train
return []message.TrainPcSimBaseMessage{{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.HANDLE_TO_ZERO, zeroState}},
{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.TRAIN_BRAKE_STATE, brakeState}},
{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.OUTER_EMERGENCY_BRAKE, 1}}}
/*{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.OUTER_EMERGENCY_BRAKE, 1}},*/
{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.NOT_BREAK, notBreak}}}
}
func (s *VerifySimulation) GetConnTrain2() []*state_proto.TrainState {
@ -491,13 +501,22 @@ func (s *VerifySimulation) reportTrainMockInitMsg(train *state_proto.TrainState,
if trainInit {
if data1 == 0 {
tcc.Line12ConnErr = true
slog.Warn("接受atp模拟量数据，data[4]=0，模拟断开")
}
if state == 0 {
train_pc_sim.Default().ResetPluseCount(train)
jjzdBtn := tcc.Buttons[JJZD]
ebTce := controlEBBtn(vobc, true, jjzdBtn)
tce = append(tce, ebTce...)
} else if message.GetBit(data1, 0) == 0 {
jjzdBtn := tcc.Buttons[JJZD]
ebTce := controlEBBtn(vobc, true, jjzdBtn)
tce = append(tce, ebTce...)
tce = append(tce, message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.OUTER_EMERGENCY_BRAKE, state}})
tce = append(tce, message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.TRAIN_TRACTION_CUTED, 1}})
//tce = append(tce, message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{message.NOT_BREAK, 0}})
//tce = append(tce, message.TrainPcSimBaseMessage{Type: message.SENDER_TRAIN_OUTR_INFO, Data: []byte{0x38, 0}})
//tce = append(tce, message.TrainPcSimBaseMessage{Type: message.RECIVE_TRAIN_HAND_KEY_CANCLE_FORWARD})
//tce = append(tce, message.TrainPcSimBaseMessage{Type: message.RECIVE_TRAIN_HAND_KEY_BACKWARD})
}
} else {
initResult = true
@ -535,6 +554,9 @@ func (s *VerifySimulation) TrainPcSimDigitalOutInfoHandle(train *state_proto.Tra
trainPcSimDigitalOutInfoHandleCode23_16(data[2], vobc)
trainPcSimDigitalOutInfoHandleCode31_24(data[1], vobc)
trainPcSimDigitalOutInfoHandleCode39_32(data[0], vobc)
/* if !vobc.EmergencyBrakingStatus {
train_pc_sim.Default().ResetPlusePlace(train)
}*/
return initResult
}
@ -674,10 +696,14 @@ func (s *VerifySimulation) TrainPcSimMockInfo(train *state_proto.TrainState, dat
train.VobcState.MockInfo = uint32(mockData)
}
var sendData []uint32
// 4.4.4. 车载输出BTM查询同步帧报文内容（0x04）
func (s *VerifySimulation) TrainBtmQuery(train *state_proto.TrainState, data []byte) {
//time.Sleep(time.Millisecond * 50)
slog.Info(fmt.Sprintf("收到车载输出BTM查询同步帧报文内容:%v", hex.EncodeToString(data)))
uuid := uuid2.NewString()
ts := time.Now().UnixMilli()
//slog.Info(fmt.Sprintf("收到车载输出BTM查询同步帧uuid:%v,时间：%v, 报文内容:%v", uuid, ts, hex.EncodeToString(data)))
if len(data) < 12 {
slog.Error("列车btm查询报文长度错误:", len(data))
return
@ -687,6 +713,7 @@ func (s *VerifySimulation) TrainBtmQuery(train *state_proto.TrainState, data []b
slog.Warn(fmt.Sprintf("列车暂时未获取到应答器信息,无法进行btm查询列车id:%v", train.Id))
return
}
notSendBtm, ok := can_btm.Default().FindNotSendState()
trainAtm := message.NewBtmHeadFrame(data)
atpReq := &message.AtpRequestFrame{}
@ -697,14 +724,20 @@ func (s *VerifySimulation) TrainBtmQuery(train *state_proto.TrainState, data []b
cl := clock(atpReq)
dsn := train_pc_sim.Default().DsnAddAndReturn()
btmRepFrame := createBtmStatus(trainAtm.CanId.ID4, train.BtmState, atpReq, cl, dsn)
//btmRepFrame := createBtmStatus(trainAtm.CanId.ID4, train.BtmState, atpReq, cl, dsn)
btmRepFrame := createBtmStatus(trainAtm.CanId.ID4, &notSendBtm, atpReq, cl, dsn)
//slog.Info(fmt.Sprintf("id1:%x,id2:%x,id3:%x,id4:%x,resendRequest:%v,reqTime:%v,dsn:%v", trainAtm.CanId.ID1, trainAtm.CanId.ID2, trainAtm.CanId.ID3, trainAtm.CanId.ID4, atpReq.ResendRequest, atpReq.Time, dsn))
if atpReq.ResendRequest == 2 {
slog.Info(fmt.Sprintf("rrrrrrrrrrrrrrrrr:%v", atpReq.String()))
//重新发送
if len(train.BtmState.BaliseTelegramForPcSimResend) > 0 {
dd, _ := hex.DecodeString(train.BtmState.BaliseTelegramForPcSimResend)
//if len(train.BtmState.BaliseTelegramForPcSimResend) > 0 {
if len(notSendBtm.BaliseTelegramForPcSimResend) > 0 {
slog.Info(fmt.Sprintf("rrrrrrrrrrrrrrrrr111111111:%v", atpReq.String()))
//dd, _ := hex.DecodeString(train.BtmState.BaliseTelegramForPcSimResend)
dd, _ := hex.DecodeString(notSendBtm.BaliseTelegramForPcSimResend)
train_pc_sim.Default().SendBaliseData(train, message.RECIVE_TRAIN_BTM_HAS_DATA, dd)
} else {
slog.Info(fmt.Sprintf("rrrrrrrrrrrrrrrrr2222222222222:%v", atpReq.String()))
timeSyncF := message.NewBtmTimeSyncCheckFrame(trainAtm.CanId.ID4)
timeSyncF.T2 = cl.BtmTk
timeSyncF.T3 = cl.TkNow()
@ -714,18 +747,31 @@ func (s *VerifySimulation) TrainBtmQuery(train *state_proto.TrainState, data []b
train_pc_sim.Default().SendBaliseData(train, message.RECIVE_TRAIN_BTM_NOT_DATA, queryData)
}
} else {
timeSyncF := message.NewBtmTimeSyncCheckFrame(trainAtm.CanId.ID4)
timeSyncF.T2 = cl.BtmTk
timeSyncF.T3 = cl.TkNow()
telCount := strings.Count(train.BtmState.Telegram, "00")
if telCount >= balise_const.UserTelegramByteLen {
//telCount := strings.Count(train.BtmState.Telegram, "00")
telCount := strings.Count(notSendBtm.Telegram, "00")
//if telCount >= balise_const.UserTelegramByteLen || train.BtmState.IsSend {
if !ok && (notSendBtm.Telegram == "" || telCount >= balise_const.UserTelegramByteLen) {
//slog.Info(fmt.Sprintf("准备发送无数据应答器id:%v", train.BtmState.BaliseId))
queryData := make([]byte, 0)
queryData = append(queryData, btmRepFrame.EncodeBtmAtp().Encode()...)
queryData = append(queryData, timeSyncF.EncodeBtmAtp().Encode()...)
train_pc_sim.Default().SendBaliseData(train, message.RECIVE_TRAIN_BTM_NOT_DATA, queryData)
} else {
//train.BtmState.IsSend = true
} else /*if !train.BtmState.IsSend*/
if !notSendBtm.IsSend {
slog.Info(fmt.Sprintf("准备发送应答id:%v,uuid :%v,接受时间:%v,发送时间:%v , 数据:%v 经过:%v,解报文：%v", notSendBtm.BaliseId, uuid, ts, time.Now().UnixMilli(), notSendBtm.Telegram, notSendBtm.BaliseCount, notSendBtm.MessageCounter))
//slog.Info(fmt.Sprintf("准备发送应答id:%v,uuid :%v,消耗时间:%v , 数据:%v", train.BtmState.BaliseId, uuid, ts-time.Now().UnixMilli(), train.BtmState.Telegram))
/* defer func() {
train.BtmState.IsSend = true
}()*/
//有数据
aliseData, _ := hex.DecodeString(train.BtmState.Telegram)
//aliseData, _ := hex.DecodeString(train.BtmState.Telegram)
aliseData, _ := hex.DecodeString(notSendBtm.Telegram)
stateRepFrame := btmRepFrame.EncodeBtmAtp()
statusDataCf, statusDataCfOk := message.CreateBtmAtpDataRspFramesData(stateRepFrame, aliseData, false, cl.BtmTk, cl.BtmTk, cl.BtmTk)
if statusDataCfOk {

1
ts/test_simulation_manage.go
View File

@ -52,6 +52,7 @@ func CreateSimulation(projectId int32, mapIds []int32, runConfig *dto.ProjectRun
}
if !e {
verifySimulation, err := memory.CreateSimulation(projectId, mapIds, runConfig)
if err != nil {
return "", err
}

BIN
北岗子-应答器报文清单.xlsx Normal file
View File

Binary file not shown.