加速度计编码调整，pc仿真添加日志
This commit is contained in:
tiger_zhou
parent
91e63a9763
commit
5df2b1c810
|
|
@ -1,12 +1,14 @@
|
|||
package main
|
||||
|
||||
import (
|
||||
"encoding/hex"
|
||||
"fmt"
|
||||
"github.com/spf13/viper"
|
||||
"joylink.club/bj-rtsts-server/bin/config"
|
||||
"joylink.club/bj-rtsts-server/third_party/message"
|
||||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||||
"log/slog"
|
||||
"sort"
|
||||
"time"
|
||||
)
|
||||
|
||||
|
|
@ -18,7 +20,8 @@ func initConfig() {
|
|||
cnf := viper.New()
|
||||
cnf.SetConfigName(config_name)
|
||||
cnf.SetConfigType("yml")
|
||||
cnf.AddConfigPath("./config/")
|
||||
cnf.AddConfigPath("./bin/config/")
|
||||
//cnf.AddConfigPath("./config/")
|
||||
cnf.AddConfigPath(".")
|
||||
err := cnf.ReadInConfig()
|
||||
if err != nil {
|
||||
|
|
@ -30,26 +33,92 @@ func initConfig() {
|
|||
}
|
||||
slog.Info("成功加载配置", "config", exampleConfig)
|
||||
}
|
||||
|
||||
func main() {
|
||||
|
||||
initConfig()
|
||||
testAcc()
|
||||
accClient := udp.NewClient(fmt.Sprintf("%v:%v", exampleConfig.Acc.RemoteIp, exampleConfig.Acc.RemotePort))
|
||||
go testAcc(accClient, 0.54)
|
||||
//btm server 接受数据测试
|
||||
ser := udp.NewServer(fmt.Sprintf("192.168.1.150:%d", 4646), handleCanetFrames)
|
||||
ser.Listen()
|
||||
fmt.Println(ser, "===========")
|
||||
|
||||
//speedClient := udp.NewClient(fmt.Sprintf("%v:%v", exampleConfig.Speed.RemoteIp, exampleConfig.Speed.RemotePort))
|
||||
//go testSpeed(speedClient, 30)
|
||||
select {}
|
||||
}
|
||||
func testAcc() {
|
||||
var client udp.UdpClient
|
||||
|
||||
func handleCanetFrames(cfs []byte) {
|
||||
defer func() {
|
||||
if client != nil {
|
||||
client.Close()
|
||||
if e := recover(); e != nil {
|
||||
slog.Debug(fmt.Sprintf("handleCanetFrames异常[%s]", e))
|
||||
}
|
||||
slog.Info("连接关闭")
|
||||
}()
|
||||
client = udp.NewClient(fmt.Sprintf("%v:%v", exampleConfig.Acc.RemoteIp, exampleConfig.Acc.RemotePort))
|
||||
ac := message.Accelerometer{Acc: 1}
|
||||
|
||||
//一个cannet 帧 13字节
|
||||
if len(cfs) > 0 && len(cfs)%13 == 0 {
|
||||
cfSum := len(cfs) / 13
|
||||
dms := make([]*message.CanetFrame, 0, 16) //13个应答器报文数据帧+TimeA帧+TimeB帧+结束帧
|
||||
for cfi := 0; cfi < cfSum; cfi++ {
|
||||
cfStart := cfi * 13
|
||||
cf := message.NewCanetFrame(cfs[cfStart:cfStart+13], false)
|
||||
//
|
||||
switch cf.CanFrameType() {
|
||||
case message.CfReq:
|
||||
fmt.Println(message.CfReq)
|
||||
case message.CfStatusRsp:
|
||||
fmt.Println(message.CfStatusRsp)
|
||||
case message.CfTimeSync:
|
||||
fmt.Println(message.CfTimeSync)
|
||||
case message.CfMsg:
|
||||
fallthrough
|
||||
case message.CfMsgTimeA:
|
||||
fallthrough
|
||||
case message.CfMsgTimeB:
|
||||
fallthrough
|
||||
case message.CfMsgEnd:
|
||||
dms = append(dms, cf)
|
||||
default:
|
||||
slog.Warn("CanetFrame帧没有具体对应的应用帧", "CannetFrame", cf.String())
|
||||
} //switch
|
||||
} //for
|
||||
//将数据包按ID3即0x80+offset由小到大排序
|
||||
sort.SliceStable(dms, func(i, j int) bool {
|
||||
return dms[i].CanId.ID3 < dms[j].CanId.ID3
|
||||
})
|
||||
//有数据帧，但是不足16帧
|
||||
if len(dms) > 0 {
|
||||
if len(dms) != 16 {
|
||||
slog.Warn("接收到数据帧，但数据帧数量不足16帧")
|
||||
} else {
|
||||
slog.Warn("接收到数据帧，但数据帧数量不足16帧111111")
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
slog.Warn("从cannet接收数据，未满足条件‘len(cfs) > 0 && len(cfs)%13 == 0‘", "len(cfs)", len(cfs), "16:", hex.EncodeToString(cfs))
|
||||
}
|
||||
}
|
||||
func recive() {
|
||||
fmt.Println("11111111111111111")
|
||||
_ = udp.NewServer(fmt.Sprintf(":%d", 4196), handleCanetFrames)
|
||||
}
|
||||
func testSpeed(client udp.UdpClient, speed float32) {
|
||||
|
||||
//client = udp.NewClient(fmt.Sprintf("%v:%v", exampleConfig.Speed.RemoteIp, exampleConfig.Speed.RemotePort))
|
||||
ac := message.ElectricMachinery{Speed: speed, WheelDiameter: 800, IsBack: false}
|
||||
var index int64
|
||||
for {
|
||||
err := client.Send(ac.Encode())
|
||||
data := ac.Encode()
|
||||
//fmt.Println(hex.EncodeToString(data))
|
||||
err := client.Send(data)
|
||||
index++
|
||||
if index%10 == 0 {
|
||||
slog.Info(fmt.Sprintf("发送数据,时间戳:%v,发送次数:%v", time.Now().Unix(), index))
|
||||
slog.Info(fmt.Sprintf("发送speed数据,时间戳:%v,发送次数:%v 数据:%v", time.Now().Unix(), index, hex.EncodeToString(data)))
|
||||
ac = message.ElectricMachinery{Speed: 0, WheelDiameter: 800, IsBack: false}
|
||||
client.Send(ac.Encode())
|
||||
break
|
||||
}
|
||||
|
||||
if err != nil {
|
||||
|
|
@ -57,5 +126,27 @@ func testAcc() {
|
|||
}
|
||||
time.Sleep(time.Millisecond * 200)
|
||||
}
|
||||
}
|
||||
|
||||
// 加速度计
|
||||
func testAcc(client udp.UdpClient, acc float32) {
|
||||
|
||||
//client = udp.NewClient(fmt.Sprintf("%v:%v", exampleConfig.Acc.RemoteIp, exampleConfig.Acc.RemotePort))
|
||||
ac := message.Accelerometer{Acc: acc}
|
||||
var index int64
|
||||
for {
|
||||
data := ac.Encode()
|
||||
//fmt.Println(hex.EncodeToString(data))
|
||||
err := client.Send(data)
|
||||
index++
|
||||
if index%10 == 0 {
|
||||
slog.Info(fmt.Sprintf("发送acc数据,时间戳:%v,发送次数:%v 数据:%v", time.Now().Unix(), index, hex.EncodeToString(data)))
|
||||
}
|
||||
|
||||
if err != nil {
|
||||
slog.Error("发送数据失败", "err", err)
|
||||
}
|
||||
time.Sleep(time.Millisecond * 500)
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,3 +1,6 @@
|
|||
acc:
|
||||
remoteIp: "127.0.0.1"
|
||||
remotePort: 1234
|
||||
remoteIp: "192.168.1.201"
|
||||
remotePort: 4196
|
||||
speed:
|
||||
remoteIp: "192.168.1.200"
|
||||
remotePort: 4196
|
||||
|
|
|
|||
|
|
@ -1,9 +1,14 @@
|
|||
package config
|
||||
|
||||
type ExampleConfig struct {
|
||||
Acc acc
|
||||
Acc acc
|
||||
Speed speed
|
||||
}
|
||||
type acc struct {
|
||||
RemoteIp string
|
||||
RemotePort int
|
||||
}
|
||||
type speed struct {
|
||||
RemoteIp string
|
||||
RemotePort int
|
||||
}
|
||||
|
|
|
|||
|
|
@ -5,6 +5,7 @@ import (
|
|||
"fmt"
|
||||
"joylink.club/bj-rtsts-server/third_party/message"
|
||||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||||
"math"
|
||||
"testing"
|
||||
)
|
||||
|
||||
|
|
@ -26,3 +27,302 @@ func handle(d []byte) {
|
|||
fmt.Println("接受数据:", string(jsonD))
|
||||
}
|
||||
}
|
||||
func TestAcc(t *testing.T) {
|
||||
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
|
||||
fmt.Println(x)
|
||||
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
|
||||
}
|
||||
|
||||
func setBit(b uint32, bitNum uint32, value uint32) uint32 {
|
||||
if value != 0 && value != 1 {
|
||||
panic("value must be 0 or 1")
|
||||
}
|
||||
mask := uint32(1 << bitNum) // 创建掩码
|
||||
b &= ^(mask) // 清除位
|
||||
b |= (mask & (value << bitNum))
|
||||
|
||||
return b
|
||||
}
|
||||
func setBit2(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
|
||||
}
|
||||
|
||||
func encode(accSpeed float32) (d2, d1, d0 byte) {
|
||||
//var accCode uint32 = math.Float32bits(accSpeed) >> 8
|
||||
var accCode uint32 = 0
|
||||
var sum float32 = accSpeed
|
||||
for i := 17; i >= 0; i-- {
|
||||
flag := float32(1 / math.Pow(2, float64(17-i)))
|
||||
//更改对应bit位数据
|
||||
if flag > sum {
|
||||
//大于加速度，设置对应的bit为0
|
||||
accCode = setBit(accCode, uint32(i), 0)
|
||||
} else {
|
||||
//设置对应的bit位1
|
||||
accCode = setBit(accCode, uint32(i), 1)
|
||||
sum = sum - flag
|
||||
}
|
||||
fmt.Println(i, flag, fmt.Sprintf("%032b", accCode), sum)
|
||||
}
|
||||
accCode = accCode << 8 //数据向左移动8位,取后3个字节
|
||||
d2 = byte(accCode >> 16) //取出d2数据
|
||||
d1 = byte(accCode >> 8 << 8) // & 0xff //取出d1
|
||||
d0 = byte(accCode << 20) //& 0xf //取出d0高2位
|
||||
|
||||
//d2 = byte(accCode >> 16) //取出d2数据
|
||||
//d1 = byte(accCode << 8 >> 8) //取出d1
|
||||
//d0 = byte(accCode << 16) //取出d0高2位
|
||||
|
||||
return d2, d1, d0
|
||||
}
|
||||
func decode(d2, d1, d0 byte) float32 {
|
||||
accCode := uint32(d2)<<16 | uint32(d1)<<8 | uint32(d0)>>4
|
||||
|
||||
// 因为编码时进行了左移8位的操作，所以这里需要右移回来
|
||||
accCode >>= 8
|
||||
|
||||
// 重建浮点数
|
||||
var accSpeed float32
|
||||
for i := 0; i <= 17; i++ {
|
||||
bitValue := accCode & (1 << uint32(i))
|
||||
if bitValue != 0 {
|
||||
// 如果该位为1，则累加相应的值到accSpeed
|
||||
accSpeed += float32(1 / math.Pow(2, float64(17-i)))
|
||||
}
|
||||
}
|
||||
|
||||
return accSpeed
|
||||
}
|
||||
func TestAccSpeed(t *testing.T) {
|
||||
|
||||
speed := float32(0.54 / G)
|
||||
fmt.Println("输入acc加速度g", speed)
|
||||
d2, d1, d0 := encode(speed)
|
||||
decode(d2, d1, d0)
|
||||
accSpeed := decode(d2, d1, d0)
|
||||
fmt.Println("输出速度:", accSpeed*G, "加速度:", accSpeed)
|
||||
}
|
||||
|
||||
func TestCode66(t *testing.T) {
|
||||
var accSpeed float32 = 0.44 / 9.8
|
||||
fmt.Println(accSpeed)
|
||||
//生成4字节返回数据
|
||||
var accCode uint32 = 0
|
||||
var sum float32 = 0
|
||||
for i := 16; i >= 0; i-- {
|
||||
flag := float32(1 / math.Pow(2, float64(17-i)))
|
||||
//更改对应bit位数据
|
||||
if flag > accSpeed {
|
||||
//大于加速度，设置对应的bit为0
|
||||
accCode = setBit(accCode, uint32(i), 0)
|
||||
} else {
|
||||
//设置对应的bit位1
|
||||
accCode = setBit(accCode, uint32(i), 1)
|
||||
if sum+flag > accSpeed {
|
||||
//累加和超过accCode停止
|
||||
break
|
||||
}
|
||||
sum = sum + flag
|
||||
}
|
||||
fmt.Println(i, flag, fmt.Sprintf("%b", accCode), sum)
|
||||
}
|
||||
//因为获取但是18位数据，这里向左移动14位（获取之前设置的bit数据）
|
||||
fmt.Println(fmt.Sprintf("%032b", accCode))
|
||||
fmt.Println(math.Float32frombits(accCode&0xffffc000) / 0x80000000)
|
||||
accCode = accCode << 14
|
||||
fmt.Println(fmt.Sprintf("%032b", accCode))
|
||||
fmt.Println(float32(accCode) / 0x80000000)
|
||||
}
|
||||
func TestCode5(t *testing.T) {
|
||||
|
||||
var accSpeed float32 = (0.54 / 9.8) * 2
|
||||
bbc := math.Float32bits(accSpeed)
|
||||
fmt.Println(accSpeed, bbc, fmt.Sprintf("%08b", bbc), float32(bbc))
|
||||
d2 := byte(bbc) & 0xFF
|
||||
d1 := byte((bbc << 8) & 0xFF)
|
||||
d0 := byte((bbc << 16) & 0xFF)
|
||||
|
||||
fmt.Println(fmt.Sprintf("%08b", d2))
|
||||
fmt.Println(fmt.Sprintf("%08b", d1))
|
||||
fmt.Println(fmt.Sprintf("%08b", d0))
|
||||
result := (uint32(d2) << 16) | (uint32(d1) << 8) | (uint32(d0))
|
||||
|
||||
fmt.Println(fmt.Sprintf("%08b", result), float32(result)/float32(0xFFFFFF), math.Float32frombits(result))
|
||||
//arr := []byte{d2, d1, d0}
|
||||
//var sum float32 = 0
|
||||
//isBreak := false
|
||||
//index := 1
|
||||
//for i := 0; i < len(arr); i++ {
|
||||
// d := arr[i]
|
||||
//
|
||||
//}
|
||||
/* for i := 22; i >= 0; i-- {
|
||||
d := float32(1 / math.Pow(2, float64(17-i)))
|
||||
if isBreak {
|
||||
result = setBit(result, uint32(i), 1)
|
||||
|
||||
} else {
|
||||
if d > accSpeed {
|
||||
result = setBit(result, uint32(i), 0)
|
||||
} else if d < accSpeed {
|
||||
result = setBit(result, uint32(i), 1)
|
||||
if sum+d > accSpeed {
|
||||
isBreak = true
|
||||
//break
|
||||
} else {
|
||||
sum += d
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fmt.Println(i, i, d, "--------", fmt.Sprintf("%8b", result), result, sum)
|
||||
}*/
|
||||
//aas := math.Float32frombits(result)
|
||||
//fmt.Println(aas)
|
||||
result = result & 0xffffc000
|
||||
fmt.Println(result, float32(result)/dd, fmt.Sprintf("%08b", result))
|
||||
fmt.Println(math.Float32frombits(result) / dd)
|
||||
}
|
||||
func TestCode3(t *testing.T) {
|
||||
var accSpeed float32 = 0.54 / 9.8
|
||||
bbc := math.Float32bits(accSpeed)
|
||||
fmt.Println(accSpeed, bbc, fmt.Sprintf("%08b", math.Float32bits(accSpeed)))
|
||||
|
||||
d2 := byte(bbc) & 0xFF
|
||||
d1 := byte((bbc << 8) & 0xFF)
|
||||
d0 := byte((bbc << 16) & 0xFF)
|
||||
//result := (uint32(d2) << 16) | (uint32(d1) << 8) | (uint32(d0))
|
||||
|
||||
//d0 := byte(bbc & 0xFF)
|
||||
// 提取 D1 字节
|
||||
//d1 := byte((bbc >> 8) & 0xFF)
|
||||
// 提取 D2 字节
|
||||
//d2 := byte((bbc >> 16) & 0xFF)
|
||||
|
||||
fmt.Println(fmt.Sprintf("%08b", d2))
|
||||
fmt.Println(fmt.Sprintf("%08b", d1))
|
||||
fmt.Println(fmt.Sprintf("%08b", d0))
|
||||
fmt.Println("----------------------------")
|
||||
d2 = setBit2(d2, 7, 0)
|
||||
d2 = setBit2(d2, 6, 0)
|
||||
d2 = setBit2(d2, 5, 0)
|
||||
d2 = setBit2(d2, 4, 0)
|
||||
d2 = setBit2(d2, 3, 0)
|
||||
d2 = setBit2(d2, 2, 0)
|
||||
d2 = setBit2(d2, 1, 0)
|
||||
d2 = setBit2(d2, 0, 1)
|
||||
|
||||
fmt.Println(fmt.Sprintf("%08b", d2))
|
||||
fmt.Println(fmt.Sprintf("%08b", d1))
|
||||
fmt.Println(fmt.Sprintf("%08b", d0))
|
||||
var result uint32
|
||||
|
||||
//arr := []byte{d2, d1, d0}
|
||||
//buf := bytes.NewBuffer(arr)
|
||||
//binary.Read(buf, binary.BigEndian, &result)
|
||||
result = (uint32(d2) << 16) | (uint32(d1) << 8) | (uint32(d0))
|
||||
aas := math.Float32frombits(result)
|
||||
fmt.Println(aas)
|
||||
result = result & 0xffffc000
|
||||
fmt.Println(result, fmt.Sprintf("%08b", result))
|
||||
fmt.Println(aas / dd)
|
||||
|
||||
}
|
||||
|
||||
func TestCode2(t *testing.T) {
|
||||
var accSpeed float32 = 0.54 / 9.8
|
||||
fmt.Println(accSpeed)
|
||||
var acc uint32 = math.Float32bits(0)
|
||||
var sum float32 = 0
|
||||
bit := 13
|
||||
for i := 16; i >= 1; i-- {
|
||||
d := float32(1 / math.Pow(2, float64(17-i)))
|
||||
if d > accSpeed {
|
||||
acc = setBit(acc, uint32(i+bit), 0)
|
||||
} else if d < accSpeed {
|
||||
acc = setBit(acc, uint32(i+bit), 1)
|
||||
if sum+d > accSpeed {
|
||||
break
|
||||
} else {
|
||||
sum += d
|
||||
}
|
||||
}
|
||||
fmt.Println(i, i+bit, d, "--------", fmt.Sprintf("%8b", acc), acc, sum)
|
||||
}
|
||||
|
||||
//d2 := byte(acc >> 24)
|
||||
//d1 := byte((acc << 8) >> 24)
|
||||
//d0 := byte((bbc << 16) >> 24)
|
||||
//arr := []byte{d2, d1, 0, 0}
|
||||
//u3 := binary.BigEndian.Uint32(arr)
|
||||
u3 := acc & 0xffffc000
|
||||
fmt.Println(u3, sum)
|
||||
aas := math.Float32frombits(u3) / 0x80000000
|
||||
fmt.Println(aas)
|
||||
}
|
||||
|
||||
const dd = 0x80000000
|
||||
|
||||
func encodeDecimal(decimal float64) ([]byte, []byte) {
|
||||
// 将小数转换为二进制形式
|
||||
binary := fmt.Sprintf("%.15f", decimal)
|
||||
|
||||
// 提取二进制小数部分
|
||||
binaryDecimal := binary[2:]
|
||||
|
||||
// 左对齐并添加零
|
||||
paddedBinaryDecimal := fmt.Sprintf("%015s", binaryDecimal)
|
||||
|
||||
// 将二进制小数转换为字节数组
|
||||
d2 := []byte(paddedBinaryDecimal[:8])
|
||||
d1 := []byte(paddedBinaryDecimal[8:])
|
||||
|
||||
return d2, d1
|
||||
}
|
||||
|
|
|
|||
|
|
@ -59,7 +59,9 @@ func (acc *accVobcService) Start(accManager AccVobcManager) {
|
|||
}
|
||||
|
||||
func (avs *accVobcService) SendAcc(acc *message.Accelerometer) {
|
||||
|
||||
avs.vobcClient.Send(acc.Encode())
|
||||
//avs.vobcClient.Send(acc.BuildCanData())
|
||||
}
|
||||
func (avs *accVobcService) TrainAccSender(info *message.DynamicsTrainInfo, trainState *state_proto.TrainState) {
|
||||
if trainState.VobcState.Tc1Active && trainState.TrainEndsA.AccEnable {
|
||||
|
|
|
|||
|
|
@ -4,6 +4,7 @@ import (
|
|||
"fmt"
|
||||
"joylink.club/bj-rtsts-server/dto/common_proto"
|
||||
"joylink.club/bj-rtsts-server/dto/state_proto"
|
||||
"log/slog"
|
||||
"sync"
|
||||
|
||||
"joylink.club/bj-rtsts-server/config"
|
||||
|
|
@ -80,7 +81,7 @@ func (s *electricalMachineryImpl) SendElectricMachineryMessage(emMap map[int]*me
|
|||
if client != nil {
|
||||
err := client.Send(em.Encode())
|
||||
if err != nil {
|
||||
fmt.Println(err)
|
||||
slog.Error(err.Error())
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ import (
|
|||
const (
|
||||
accHeader = 0xA6
|
||||
uaidDefault = 0x1C
|
||||
G = 9.80665
|
||||
)
|
||||
|
||||
type Accelerometer struct {
|
||||
|
|
@ -21,23 +22,66 @@ const (
|
|||
ReadingMask = 0xFFFFC000 // 屏蔽标志位和编码位
|
||||
)
|
||||
|
||||
func encodeAcc(a float32) (d2, d1, d0 byte) {
|
||||
d2 = 0
|
||||
d1 = 0
|
||||
d0 = 0
|
||||
x := a / G
|
||||
//fmt.Println(x)
|
||||
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
|
||||
}
|
||||
|
||||
func (acc *Accelerometer) BuildCanData() []byte {
|
||||
accData := acc.Encode()
|
||||
appData := make([]byte, 0)
|
||||
appData = append(appData, accData...)
|
||||
appData = append(appData, byte(0))
|
||||
canId := CanFrameId{ID1: 0xff, ID2: 0xff, ID3: 0xff, ID4: 0xff}
|
||||
canNet := &CanetFrame{FF: false, RTR: false, CanLen: 7, CanId: canId, CanData: appData, IsTrainPcSim: false}
|
||||
return canNet.Encode()
|
||||
}
|
||||
func (acc *Accelerometer) Encode() []byte {
|
||||
accData := uint32(math.Round(float64(acc.Acc) * float64(FullScaleG)))
|
||||
d2 := byte(accData >> 16)
|
||||
d1 := byte(accData >> 8)
|
||||
D0 := byte(accData) & 0xFF
|
||||
D0 |= byte((accData >> 18) & DataMask)
|
||||
d2, d1, d0 := encodeAcc(acc.Acc)
|
||||
data := make([]byte, 0)
|
||||
data = append(data, accHeader)
|
||||
data = append(data, uaidDefault)
|
||||
data = append(data, D0)
|
||||
data = append(data, d0)
|
||||
data = append(data, d1)
|
||||
data = append(data, d2)
|
||||
data = append(data, acc.Aux)
|
||||
cs := checkSum(data)
|
||||
data = append(data, ^cs+1)
|
||||
return data
|
||||
|
||||
}
|
||||
|
||||
func checkSum(data []byte) byte {
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ func (t *ElectricMachinery) Encode() []byte {
|
|||
//b = binary.BigEndian.AppendUint32(b, uint32(rotarySpeed))
|
||||
rotarySpeedData = uint32(rotarySpeed)
|
||||
}
|
||||
//fmt.Println(rotarySpeed, rotarySpeedData)
|
||||
b = binary.BigEndian.AppendUint32(b, rotarySpeedData)
|
||||
crc := chkcrc(b) // crc校验码
|
||||
b = binary.BigEndian.AppendUint16(b, crc)
|
||||
|
|
|
|||
|
|
@ -193,6 +193,7 @@ func (d *trainPcSimService) initConn(clientKey string) {
|
|||
fmt.Println(properties[CLIENT_KEY])
|
||||
client2, err := tcp.StartTcpClient(addr, properties, d.reivceData, d.readError)
|
||||
if err != nil {
|
||||
slog.Error("车载pc连接失败 clientKey:", clientKey, "error:", err.Error())
|
||||
d.updateState(tpapi.ThirdPartyState_Broken)
|
||||
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -362,7 +362,7 @@ func (s *VerifySimulation) TrainPcSimDigitalOutInfoHandle(connType state_proto.T
|
|||
//vobc.BrakeQuarantine = message.IsTrueForByte(brakeQuarantine) //? //制动隔离
|
||||
//vobc.StopNotAllBrake = message.IsTrueForByte(stopNotAllBrake) //? //停放制动缓解
|
||||
trainPcSimDigitalOutInfoHandleCode39_32(data[0], vobc)
|
||||
trainPcSimDigitalOutInfoHandleCode31_24(data[1], vobc)
|
||||
trainPcSimDigitalOutInfoHandleCode31_24(connType, data[1], vobc)
|
||||
trainPcSimDigitalOutInfoHandleCode23_16(data[2], vobc)
|
||||
trainPcSimDigitalOutInfoHandleCode15_8(data[3], vobc)
|
||||
trainPcSimDigitalOutInfoHandleCode7_0(data[4], vobc)
|
||||
|
|
@ -380,15 +380,20 @@ func trainPcSimDigitalOutInfoHandleCode39_32(d byte, vobc *state_proto.TrainVobc
|
|||
vobc.StopNotAllBrake = message.IsTrueForByte(message.GetBit(d, 7)) //? 停放制动缓解
|
||||
|
||||
}
|
||||
func trainPcSimDigitalOutInfoHandleCode31_24(d byte, vobc *state_proto.TrainVobcState) {
|
||||
func trainPcSimDigitalOutInfoHandleCode31_24(connType state_proto.TrainConnState_TrainConnType, d byte, vobc *state_proto.TrainVobcState) {
|
||||
vobc.AtoOpenLeftDoor = message.IsTrueForByte(message.GetBit(d, 0)) //?//ATO开左门
|
||||
vobc.AtoOpenRightDoor = message.IsTrueForByte(message.GetBit(d, 1)) //?//ATO开右门
|
||||
vobc.AtoCloseLeftDoor = message.IsTrueForByte(message.GetBit(d, 2)) //?//ATO关左门
|
||||
vobc.Tc1Active = message.IsTrueForByte(message.GetBit(d, 3)) //驾驶室激活
|
||||
vobc.NoSpeedSigle = message.IsTrueForByte(message.GetBit(d, 4)) //?//零速信号
|
||||
vobc.Fam = message.IsTrueForByte(message.GetBit(d, 5)) //FAM模式
|
||||
vobc.Cam = message.IsTrueForByte(message.GetBit(d, 6)) //CAM模式
|
||||
vobc.TrainStartedLed = message.IsTrueForByte(message.GetBit(d, 7)) //?//列车启动指示灯
|
||||
if connType == state_proto.TrainConnState_PC_SIM_A {
|
||||
vobc.Tc1Active = message.IsTrueForByte(message.GetBit(d, 3))
|
||||
} else {
|
||||
vobc.Tc2Active = message.IsTrueForByte(message.GetBit(d, 3))
|
||||
}
|
||||
//驾驶室激活
|
||||
vobc.NoSpeedSigle = message.IsTrueForByte(message.GetBit(d, 4)) //?//零速信号
|
||||
vobc.Fam = message.IsTrueForByte(message.GetBit(d, 5)) //FAM模式
|
||||
vobc.Cam = message.IsTrueForByte(message.GetBit(d, 6)) //CAM模式
|
||||
vobc.TrainStartedLed = message.IsTrueForByte(message.GetBit(d, 7)) //?//列车启动指示灯
|
||||
|
||||
}
|
||||
func trainPcSimDigitalOutInfoHandleCode23_16(d byte, vobc *state_proto.TrainVobcState) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue