329 lines
8.2 KiB
Go
329 lines
8.2 KiB
Go
package acc
|
||
|
||
import (
|
||
"encoding/json"
|
||
"fmt"
|
||
"joylink.club/bj-rtsts-server/third_party/message"
|
||
"joylink.club/bj-rtsts-server/third_party/udp"
|
||
"math"
|
||
"testing"
|
||
)
|
||
|
||
func TestAccUdp(t *testing.T) {
|
||
|
||
fmt.Println("准备启动ACC服务...")
|
||
addr := fmt.Sprintf("%v:%v", "127.0.0.1", "9998")
|
||
server := udp.NewServer(addr, handle)
|
||
server.Listen()
|
||
select {}
|
||
}
|
||
|
||
func handle(d []byte) {
|
||
ri := message.Accelerometer{}
|
||
err := ri.Decode(d)
|
||
if err == nil {
|
||
|
||
jsonD, _ := json.Marshal(ri)
|
||
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
|
||
}
|