Merge remote-tracking branch 'origin/develop' into develop
# Conflicts: # rts-sim-module
This commit is contained in:
commit
844560ae1e
@ -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,
|
||||
|
@ -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) {
|
||||
|
@ -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
|
@ -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() {
|
||||
|
File diff suppressed because it is too large
Load Diff
406
example/balise_1023_830/main.go
Normal file
406
example/balise_1023_830/main.go
Normal 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
156
example/balise_1023_830/read_excel.go
Normal 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
32
example/ex1/main.go
Normal 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
56
example/lfsr/main.go
Normal 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
52
example/test/main.go
Normal 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
6
go.mod
@ -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
13
go.sum
@ -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=
|
||||
|
@ -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=
|
||||
|
@ -1 +1 @@
|
||||
Subproject commit 86c02883c4993bf829599c7516862009077d586b
|
||||
Subproject commit efa7d1655ed781512c8b0ce56685954fa3cc9903
|
184
third_party/balise/codec.go
vendored
184
third_party/balise/codec.go
vendored
@ -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
113
third_party/balise/conv.go
vendored
@ -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
2
third_party/balise/conv_test.go
vendored
@ -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
88
third_party/balise/decode.go
vendored
Normal 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
86
third_party/balise/encode.go
vendored
Normal 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
63
third_party/can_btm/balise_btm.go
vendored
@ -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
137
third_party/can_btm/balise_detection.go
vendored
@ -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
4
third_party/example/main.go
vendored
@ -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
10
third_party/message/dynamics.go
vendored
@ -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
2
third_party/message/radar.go
vendored
@ -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
41
third_party/message/train_pc_sim_message.go
vendored
@ -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
99
third_party/train_pc_sim/train_pc_sim.go
vendored
@ -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)))
|
||||
|
@ -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 {
|
||||
|
@ -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,10 +418,14 @@ 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
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// 根据动力学修改列车位置
|
||||
@ -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
|
||||
|
||||
|
@ -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, ¬SendBtm, 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 {
|
||||
|
@ -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
BIN
北岗子-应答器报文清单.xlsx
Normal file
Binary file not shown.
Loading…
Reference in New Issue
Block a user