雷达发送功能

bj-rtss-message

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

config/config.go

@@ -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"` // 连接地址

example/mock_train_service/main.go

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

third_party/message/radar.go

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

third_party/message/radar_test.go

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

third_party/radar/radar_test.go

@@ -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))
+	}
+}

third_party/radar/radar_vobc.go

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

ts/simulation/wayside/memory/wayside_simulation.go

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

ts/test_simulation_manage.go

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