rts-sim-testing-service/third_party/acc/acc_server_test.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
}