rts-sim-testing-service/third_party/message/train_control.go

package message

import (
	"encoding/binary"
	"joylink.club/bj-rtsts-server/dto/state_proto"
	"math"
	"strconv"
	"time"
)

// 接收到的列车控制信息
type TrainControlMsg struct {
	ControlInfo *state_proto.TrainVobcState
	TrainId     string
	FromVobc    bool
}

func boolsToByte2(flags [8]bool) byte {
	var result uint8
	for index, b := range flags {
		if b {
			result = result + (1 << index)
		}
	}
	return result
}

func (r *TrainControlMsg) Encode() []byte {
	if !r.FromVobc {
		ls := r.ControlInfo.LifeSignal
		r.ControlInfo.LifeSignal = ls + 1
		r.ControlInfo.UpdateTime = time.Now().UnixMilli()
	}
	//data := make([]byte, 0)
	//data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.LifeSignal))
	//var b2 uint8
	//b2 = setBit(b2, 0, IsTrue(r.ControlInfo.Tc1Active))
	//b2 = setBit(b2, 1, IsTrue(r.ControlInfo.Tc2Active))
	//b2 = setBit(b2, 2, IsTrue(r.ControlInfo.DirectionForward))
	//b2 = setBit(b2, 3, IsTrue(r.ControlInfo.DirectionBackward))
	//b2 = setBit(b2, 4, IsTrue(r.ControlInfo.TractionStatus))
	//b2 = setBit(b2, 5, IsTrue(r.ControlInfo.BrakingStatus))
	//b2 = setBit(b2, 6, IsTrue(r.ControlInfo.EmergencyBrakingStatus))
	//b2 = setBit(b2, 7, IsTrue(r.ControlInfo.TurnbackStatus))
	//fmt.Println(fmt.Sprintf("%b", b2))
	//var b3 uint8
	//b3 = setBit(b3, 0, IsTrue(r.ControlInfo.JumpStatus))
	//b3 = setBit(b3, 1, IsTrue(r.ControlInfo.Ato))
	//b3 = setBit(b3, 2, IsTrue(r.ControlInfo.Fam))
	//b3 = setBit(b3, 3, IsTrue(r.ControlInfo.Cam))
	//b3 = setBit(b3, 4, IsTrue(r.ControlInfo.TractionSafetyCircuit))
	//b3 = setBit(b3, 5, IsTrue(r.ControlInfo.ParkingBrakeStatus))
	//b3 = setBit(b3, 6, IsTrue(r.ControlInfo.MaintainBrakeStatus))
	//b3 = setBit(b3, 7, IsTrue(true))
	//data = append(data, b2)
	//data = append(data, b3)
	//data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.TractionForce))
	//data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.BrakeForce))
	//data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.TrainLoad))
	//data = binary.BigEndian.AppendUint16(data, uint16(0)) //预留
	//data = binary.BigEndian.AppendUint16(data, uint16(0)) //预留
	//data = append(data, uint8(0))                         //预留
	//var b4 byte
	//b4 = setBit(b4, 0, IsTrue(r.ControlInfo.LeftDoorOpenCommand))
	//b4 = setBit(b4, 1, IsTrue(r.ControlInfo.RightDoorOpenCommand))
	//b4 = setBit(b4, 2, IsTrue(r.ControlInfo.LeftDoorCloseCommand))
	//b4 = setBit(b4, 3, IsTrue(r.ControlInfo.RightDoorCloseCommand))
	//b4 = setBit(b4, 4, IsTrue(r.ControlInfo.AllDoorClose))
	//b4 = setBit(b4, 5, IsTrue(false))
	//b4 = setBit(b4, 6, IsTrue(false))
	//b4 = setBit(b4, 7, IsTrue(false))
	//data = append(data, b4)
	//data = binary.BigEndian.AppendUint16(data, 0) //预留
	//data = append(data, uint8(0))                 //预留
	//data = append(data, uint8(0))                 //预留
	//ti, _ := strconv.Atoi(r.TrainId)
	//data = append(data, byte(ti))
	//return data
	var data []byte
	data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.LifeSignal))
	data = append(data, boolsToByte([8]bool{
		r.ControlInfo.Tc1Active, r.ControlInfo.Tc2Active, r.ControlInfo.DirectionForward, r.ControlInfo.DirectionBackward,
		r.ControlInfo.TractionStatus, r.ControlInfo.BrakingStatus, r.ControlInfo.EmergencyBrakingStatus, r.ControlInfo.TurnbackStatus,
	}))
	data = append(data, boolsToByte([8]bool{
		r.ControlInfo.JumpStatus, r.ControlInfo.Ato, r.ControlInfo.Fam, r.ControlInfo.Cam,
		r.ControlInfo.TractionSafetyCircuit, r.ControlInfo.ParkingBrakeStatus, r.ControlInfo.MaintainBrakeStatus, false,
	}))
	data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.TractionForce))
	data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.BrakeForce))
	data = binary.BigEndian.AppendUint16(data, uint16(r.ControlInfo.TrainLoad))
	// 预留
	data = binary.BigEndian.AppendUint16(data, 0)
	data = binary.BigEndian.AppendUint16(data, 0)
	// 预留
	data = append(data, uint8(0))
	data = append(data, boolsToByte([8]bool{
		r.ControlInfo.LeftDoorOpenCommand, r.ControlInfo.RightDoorOpenCommand, r.ControlInfo.LeftDoorCloseCommand, r.ControlInfo.RightDoorCloseCommand,
		r.ControlInfo.AllDoorClose, false, false, false,
	}))
	// 预留
	data = binary.BigEndian.AppendUint16(data, 0)
	data = append(data, uint8(0))
	data = append(data, uint8(0))
	ti, _ := strconv.Atoi(r.TrainId)
	data = append(data, byte(ti))
	return data
}
func GetBit(b byte, bitNum uint) byte {
	return (b >> bitNum) & 1
}
func setBit(b byte, bitNum int, value byte) byte {
	if value != 0 && value != 1 {
		panic("value must be 0 or 1")
	}
	mask := byte(1 << uint(bitNum)) // 创建掩码
	b &= ^(mask)                    // 清除位
	b |= (mask & (value << uint(bitNum)))
	return b
}

// 解析VOBC列车信息
func (r *TrainControlMsg) Decode(buf []byte) error {
	t := &state_proto.TrainVobcState{}
	t.LifeSignal = int32(binary.BigEndian.Uint16(buf[0:2]))
	b2 := buf[2]
	t.Tc1Active = (b2 & 1) != 0
	t.Tc2Active = (b2 & (1 << 1)) != 0
	t.DirectionForward = (b2 & (1 << 2)) != 0
	t.DirectionBackward = (b2 & (1 << 3)) != 0
	t.TractionStatus = (b2 & (1 << 4)) != 0
	t.BrakingStatus = (b2 & (1 << 5)) != 0
	t.EmergencyBrakingStatus = (b2 & (1 << 6)) != 0
	t.TurnbackStatus = (b2 & (1 << 7)) != 0
	b3 := buf[3]
	t.JumpStatus = (b3 & 1) != 0
	t.Ato = (b3 & (1 << 1)) != 0
	t.Fam = (b3 & (1 << 2)) != 0
	t.Cam = (b3 & (1 << 3)) != 0
	t.TractionSafetyCircuit = (b3 & (1 << 4)) != 0
	t.ParkingBrakeStatus = (b3 & (1 << 5)) != 0
	t.MaintainBrakeStatus = (b3 & (1 << 6)) != 0
	t.TractionForce = int64(binary.BigEndian.Uint16(buf[4:6]))
	t.BrakeForce = int64(binary.BigEndian.Uint16(buf[6:8]))
	t.TrainLoad = int64(binary.BigEndian.Uint16(buf[8:10]))
	b4 := buf[15]
	t.LeftDoorOpenCommand = (b4 & 1) != 0
	t.RightDoorOpenCommand = (b4 & (1 << 1)) != 0
	t.LeftDoorCloseCommand = (b4 & (1 << 2)) != 0
	t.RightDoorCloseCommand = (b4 & (1 << 3)) != 0
	t.AllDoorClose = (b4 & (1 << 4)) != 0
	t.UpdateTime = time.Now().UnixMilli()
	r.ControlInfo = t
	return nil
}

// 发送列车信息
type TrainSpeedMsg struct {
	// 生命信号	每个周期+1
	lifeSignal uint16
	// 列车速度	10=1km/h
	speed uint16
	// 上坡
	upslope bool
	// 坡度值 1= 1‰
	slope uint16
	// 加速度 100 = 1 m/s*s
	acceleration uint8
	// 减速度 100 = 1 m/s*s
	deceleration uint8
	// 实际运行阻力 100 = 1KN
	totalResistance uint32
	// 空气阻力 100 = 1KN
	airResistance uint32
	// 坡道阻力 100 = 1KN
	slopeResistance uint32
	// 曲线阻力 100 = 1KN
	curveResistance uint32
}

func (t *TrainSpeedMsg) DynamicsDecode(info *DynamicsTrainInfo) {
	t.lifeSignal = info.LifeSignal
	t.speed = uint16(math.Abs(float64(info.Speed * 36)))
	t.upslope = info.UpSlope
	t.slope = uint16(info.Slope)
	t.totalResistance = uint32(math.Abs(float64(info.TotalResistance / 10)))
	t.airResistance = uint32(info.AirResistance / 10)
	t.slopeResistance = uint32(math.Abs(float64(info.SlopeResistance / 10)))
	t.curveResistance = uint32(info.CurveResistance / 10)
	d := math.Abs(float64(info.Acceleration * 100))
	if info.Acceleration > 0 {
		t.acceleration = uint8(d)
	} else {
		t.deceleration = uint8(d)
	}
}

func (t *TrainSpeedMsg) Encode() []byte {
	var data []byte
	data = binary.BigEndian.AppendUint16(data, t.lifeSignal)
	data = binary.BigEndian.AppendUint16(data, t.speed)
	if t.upslope {
		data = append(data, 1)
	} else {
		data = append(data, 0)
	}
	// 中间预留一位
	data = append(data, 0)
	data = append(data, t.acceleration)                           // 加速度 100 = 1 m/s*s
	data = append(data, t.deceleration)                           // 减速度 100 = 1 m/s*s
	data = binary.BigEndian.AppendUint32(data, t.totalResistance) // 实际运行阻力 100 = 1KN
	data = binary.BigEndian.AppendUint32(data, t.airResistance)   // 空气阻力 100 = 1KN
	data = binary.BigEndian.AppendUint32(data, t.slopeResistance) // 坡道阻力 100 = 1KN
	data = binary.BigEndian.AppendUint32(data, t.curveResistance) // 曲线阻力 100 = 1KN
	data = binary.BigEndian.AppendUint16(data, t.slope)           // 坡度值 1= 1‰
	return data
}