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

package message

import (
	"fmt"
	"math"
)

const (
	accHeader   = 0xA6
	uaidDefault = 0x1C
)

type Accelerometer struct {
	Acc float32
	Aux byte
}

const (
	FullScaleG  = 0x80000000 // 1g的Q31表示
	DataMask    = 0xC0       // 最高2位是数据位
	ReadingMask = 0xFFFFC000 // 屏蔽标志位和编码位
)

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)
	data := make([]byte, 0)
	data = append(data, accHeader)
	data = append(data, uaidDefault)
	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 {
	var sum byte = 0
	for _, d := range data {
		sum += d
	}
	return sum
}

func (acc *Accelerometer) Decode(Packet []byte) error {
	if len(Packet) < 7 {
		return fmt.Errorf("")
	}
	data := uint32(Packet[4])<<16 | uint32(Packet[3])<<8 | uint32(Packet[2])
	// 应用屏蔽以移除标志位和编码位
	data &= ReadingMask
	// 将Q31值转换为float32加速度值（g）
	acceleration := float32(float32(data) * (1.0 / float32(FullScaleG)))

	fmt.Println(acceleration)
	return nil
}