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

雷达发送功能

This commit is contained in:
tiger_zhou 2024-01-24 17:12:34 +08:00
parent cb101453df
commit 5ee14ba52e
9 changed files with 226 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
bj-rtss-message

@ -1 +1 @@
Subproject commit 7b637f0f519e0c6f5fa6817546f70000ac22fd10
Subproject commit e800ded2c3b80a73cab3efc07c7146b7ae48035a

8
config/config.go
View File

@ -69,8 +69,14 @@ type ThridPartyConfig struct {
ElectricMachinery ElectricMachineryConfig `json:"electricMachinery" description:"电机配置"`
BtmCanet BtmCanetConfig `json:"btmCanet" description:"BTM关联的网关设备CANET配置"`
CidcModbus []CidcModbusConfig `json:"cidcModbus" description:"联锁驱采Modbus接口配置"`
Radar RadarConfig `json:"radar" description:"车载雷达相关配置"`
}
type RadarConfig struct {
Open bool `json:"open" description:"是否开启"`
RemoteIp string `json:"remoteIp" description:"远端接收列车信息ip"`
RemotePort uint32 `json:"remotePort" description:"远端接收列车信息端口"`
//LocalPort uint32 `json:"localPort" description:"本地端口"`
}
type CidcModbusConfig struct {
Open bool `json:"open" description:"是否开启"`
Url string `json:"url" description:"接口URL【格式tcp://{ip}:{port}】" default:"tcp://127.0.0.1:502"` // 连接地址

6
example/mock_train_service/main.go
View File

@ -21,8 +21,10 @@ var ti = &ReceiveTrainInfo{
var trainRun bool
var IP = net.IPv4(192, 168, 3, 7)
var SendIP = net.IPv4(192, 168, 3, 7)
// var IP = net.IPv4(192, 168, 3, 7)
// var SendIP = net.IPv4(192, 168, 3, 7)
var IP = net.IPv4(127, 0, 0, 1)
var SendIP = net.IPv4(127, 0, 0, 1)
func main() {
listen, err := net.ListenUDP("udp", &net.UDPAddr{

84
third_party/message/radar.go vendored
View File

@ -5,6 +5,7 @@ import (
"encoding/binary"
"fmt"
"strings"
"sync/atomic"
)
const (
@ -12,22 +13,20 @@ const (
radar_head2 byte = 0xFC
)
var autoInc = atomic.Int32{}
// 雷达与VOBC接口-雷达通讯协议
type Radar struct {
AutoInc byte //自增计数器，每发送一次自增1.范围0-256
RealSpeed *RadarData //实际速度
DriftCounterS1 *RadarData //位移计数器S1
DriftCounterS2 *RadarData //位移计数器S1
InnerCheck1 byte //内部使用，我们只有在协议效验时用到该两个字节
InnerCheck2 byte //内部使用，我们只有在协议效验时用到该两个字节
type RadarInfo struct {
AutoInc byte //自增计数器，每发送一次自增1.范围0-256
RealSpeed uint16 //实际速度
DriftCounterS1 uint16 //位移计数器S1
DriftCounterS2 uint16 //位移计数器S2
InnerCheck1 byte //内部使用，我们只有在协议效验时用到该两个字节
InnerCheck2 byte //内部使用，我们只有在协议效验时用到该两个字节
State *RadarState
Tail byte
}
type RadarData struct {
SourceData byte //接收源数据
data uint16 //移位后的数据
valRange byte //数据取值范围
}
type RadarState struct {
SourceState byte //原数据
Model string // 天线模式
@ -36,7 +35,34 @@ type RadarState struct {
Dir string //方向
}
func (r *Radar) Decode(data []byte) error {
func (r RadarInfo) Encode() []byte {
buf := make([]byte, 0)
buf = append(buf, radar_head1)
buf = append(buf, radar_head2)
if tmp := autoInc.Add(1); tmp >= 256 {
autoInc.Store(0)
buf = append(buf, 0)
} else {
buf = append(buf, byte(tmp))
}
buf = binary.LittleEndian.AppendUint16(buf, r.RealSpeed)
buf = binary.LittleEndian.AppendUint16(buf, r.DriftCounterS1)
buf = binary.LittleEndian.AppendUint16(buf, r.DriftCounterS2)
buf = append(buf, 0)
buf = append(buf, 0)
//6,7位 11
//3,4,5位 011
// 1位 1
//0位 1
buf = append(buf, 0|(byte(1)<<7)|(byte(1)<<6)|(byte(1)<<5)|(byte(1)<<4)|(byte(1)<<1)|(byte(1)<<0))
var sum int = 0
for _, d := range buf {
sum += int(d)
}
buf = append(buf, byte(^sum+1))
return buf
}
func (r *RadarInfo) Decode(data []byte) error {
if len(data) < 13 {
return fmt.Errorf("雷达数据预读取失败，需要读取13字节，可读取:%v", len(data))
}
@ -61,15 +87,12 @@ func (r *Radar) Decode(data []byte) error {
r.InnerCheck2 = i2
r.State = state
r.Tail = tail
if !r.checkTail() {
if !(r.Tail == r.createTail()) {
return fmt.Errorf("数据解析完成，但协议效验不通过")
}
state.parseState()
return nil
}
func (rd *RadarData) getSumVal() byte {
return rd.SourceData + rd.valRange
}
func (s *RadarState) parseState() {
//第7位 == SW_Mode0, 第6位 == SW_Mode1
// 11:两个天线和双通道都OK
@ -119,10 +142,22 @@ func bitStateStr(data []byte) string {
}*/
return build.String()
}
func (r *Radar) checkTail() bool {
var sum = int(radar_head1) + int(radar_head2) + int(r.AutoInc+r.RealSpeed.getSumVal()+r.DriftCounterS1.getSumVal()+r.DriftCounterS2.getSumVal()+r.InnerCheck1+r.InnerCheck2+r.State.SourceState)
return r.Tail == byte(^sum+1)
func culDataSize(d uint16) int {
return int(d>>8) + int(d&0x00FF)
}
func (r *RadarInfo) createTail() byte {
var sum = int(radar_head1) + int(radar_head2)
sum += int(r.AutoInc)
sum += culDataSize(r.RealSpeed)
sum += culDataSize(r.DriftCounterS1)
sum += culDataSize(r.DriftCounterS2)
sum += int(r.InnerCheck1)
sum += int(r.InnerCheck2)
sum += int(r.State.SourceState)
return byte(^sum + 1)
}
func readHeader(buf *bytes.Buffer) (byte, byte, error) {
/*if buf.Len() < 2 {
return 0, 0, fmt.Errorf("雷达协议解析头部没有可读充足的数据")
@ -142,13 +177,12 @@ func readByteData(buf *bytes.Buffer) byte {
d, _ := buf.ReadByte()
return d
}
func readSpeedOrCounter(buf *bytes.Buffer) *RadarData {
/* if buf.Len() < 2 {
return nil, fmt.Errorf("")
}*/
func readSpeedOrCounter(buf *bytes.Buffer) uint16 {
ss, _ := buf.ReadByte()
limit, _ := buf.ReadByte()
return &RadarData{SourceData: ss, valRange: limit, data: uint16(ss) << 8}
data := binary.LittleEndian.Uint16([]byte{ss, limit})
return data
//return &RadarData{SourceData: ss, valRange: limit, data: data}
}
func readRadarInnerData(buf *bytes.Buffer) (byte, byte) {

47
third_party/message/radar_test.go vendored
View File

@ -1,47 +0,0 @@
package message
import (
"fmt"
"testing"
)
func TestRadar(t *testing.T) {
data := make([]byte, 0)
data = append(data, radar_head1) //0
data = append(data, radar_head2) //1
data = append(data, 1) //自增
data = append(data, 1) //速度3
data = append(data, 0) //4
data = append(data, 1) //s1 5
data = append(data, 0) //6
data = append(data, 1) //s2 7
data = append(data, 0) //8
data = append(data, 0) //内部使用 9
data = append(data, 0) //内部使用 9
data = append(data, createState())
var sum int
for _, s := range data {
sum += int(s)
}
data = append(data, byte(^sum+1))
radar := &Radar{}
radar.Decode(data)
fmt.Println(radar)
}
func createState() byte {
var b byte = 0
b1 := b | (byte(1) << 7) | (byte(1) << 6) | (byte(1) << 5) | (byte(1) << 4) | (byte(1) << 1) | (byte(1) << 1)
return b1
}
func TestR1(t *testing.T) {
var b byte = 0
fmt.Printf("%08b\n", b)
//6,7位 11
//3,4,5位 011
// 1位 1
//0位 1
b1 := b | (byte(1) << 7) | (byte(1) << 6) | (byte(1) << 5) | (byte(1) << 4) | (byte(1) << 1) | (byte(1) << 0)
fmt.Printf("%08b\n", b1)
fmt.Println(b1)
}

28
third_party/radar/radar_test.go vendored Normal file
View File

@ -0,0 +1,28 @@
package radar
import (
"encoding/json"
"fmt"
"joylink.club/bj-rtsts-server/third_party/message"
"joylink.club/bj-rtsts-server/third_party/udp"
"testing"
)
func TestUdp(t *testing.T) {
fmt.Println("准备启动服务...")
addr := fmt.Sprintf("%v:%v", "127.0.0.1", "8899")
server := udp.NewServer(addr, handle)
server.Listen()
for {
}
}
func handle(d []byte) {
ri := message.RadarInfo{}
err := ri.Decode(d)
if err == nil {
jsonD, _ := json.Marshal(ri)
fmt.Println(string(jsonD))
}
}

97
third_party/radar/radar_vobc.go vendored Normal file
View File

@ -0,0 +1,97 @@
package radar
import (
"context"
"fmt"
"joylink.club/bj-rtsts-server/config"
"joylink.club/bj-rtsts-server/dto/state_proto"
"joylink.club/bj-rtsts-server/third_party/message"
"joylink.club/bj-rtsts-server/third_party/udp"
"log/slog"
"math"
"sync"
"time"
)
type RadarVobc interface {
Start(radar RadarVobcManager)
Stop()
SendRadarInfo(ctx context.Context)
}
type RadarVobcManager interface {
GetRunRadarConfig() config.RadarConfig
FindRadarTrain() *state_proto.TrainState
}
const radar_interval = 15
var (
initLock = sync.Mutex{}
_radar *radarVobc
fixed_speed = 0.009155
)
func Default() RadarVobc {
defer func() {
initLock.Unlock()
}()
initLock.Lock()
if _radar == nil {
_radar = &radarVobc{}
}
return _radar
}
type radarVobc struct {
radarVobcTaskContext context.CancelFunc
vobcClient udp.UdpClient
radarVobcManager RadarVobcManager
}
func (rv *radarVobc) Start(radar RadarVobcManager) {
config := radar.GetRunRadarConfig()
if config.RemoteIp == "" || config.RemotePort == 0 || !config.Open {
slog.Info("雷达未开启", "远端ip:", config.RemoteIp, "远端端口:", config.RemotePort, "是否开启:", config.Open)
return
}
rv.vobcClient = udp.NewClient(fmt.Sprintf("%v:%v", config.RemoteIp, config.RemotePort))
ctx, cancleFunc := context.WithCancel(context.Background())
rv.radarVobcTaskContext = cancleFunc
rv.radarVobcManager = radar
go rv.SendRadarInfo(ctx)
}
func (rv *radarVobc) SendRadarInfo(ctx context.Context) {
/*defer func() {
slog.Error("")
}()*/
for {
select {
case <-ctx.Done():
return
default:
}
trainStatus := rv.radarVobcManager.FindRadarTrain()
if trainStatus != nil {
hourSpeed := float64(trainStatus.DynamicState.Speed / 100)
ri := message.RadarInfo{RealSpeed: uint16(math.Round(hourSpeed / fixed_speed)), DriftCounterS1: 0, DriftCounterS2: 0}
rv.vobcClient.SendMsg(ri)
time.Sleep(time.Millisecond * radar_interval)
}
}
}
func (rv *radarVobc) Stop() {
if rv.vobcClient != nil {
rv.vobcClient.Close()
rv.vobcClient = nil
}
if rv.radarVobcTaskContext != nil {
rv.radarVobcTaskContext()
rv.radarVobcTaskContext = nil
}
}

25
ts/simulation/wayside/memory/wayside_simulation.go
View File

@ -535,6 +535,31 @@ func (s *VerifySimulation) initRepository() error {
return nil
}
// GetRunRadarConfig 获取雷达配置信息
func (s *VerifySimulation) GetRunRadarConfig() config.RadarConfig {
return s.runConfig.Radar
}
// FindRadarTrain 查找一个列车 只有1端雷达开启啊
func (s *VerifySimulation) FindRadarTrain() *state_proto.TrainState {
var trainStatus *state_proto.TrainState
s.Memory.Status.TrainStateMap.Range(func(k any, v any) bool {
val, ok := v.(*state_proto.TrainState)
if ok {
if val.TrainEndsA.RadarEnable && val.TrainEndsB.RadarEnable {
//trainStatus = val
//return false
return true
} else if val.TrainEndsA.RadarEnable || val.TrainEndsB.RadarEnable {
trainStatus = val
return false
}
}
return true
})
return trainStatus
}
func buildProtoRepository(mapIds []int32) (*proto.Repository, error) {
repo := &proto.Repository{}
var exceptStationGiMapIds []int32

5
ts/test_simulation_manage.go
View File

@ -2,6 +2,7 @@ package ts
import (
"fmt"
"joylink.club/bj-rtsts-server/third_party/radar"
"log/slog"
"runtime"
"strconv"
@ -130,6 +131,8 @@ func runThirdParty(s *memory.VerifySimulation) error {
if err != nil {
return err
}
//列车雷达发送vobc
radar.Default().Start(s)
return nil
}
@ -151,6 +154,8 @@ func stopThirdParty(s *memory.VerifySimulation) {
can_btm.Default().Stop()
// 联锁驱采Modbus服务停止
cidcmodbus.Stop()
// 雷达服务停止
radar.Default().Stop()
}
func createSimulationId(projectId int32) string {