64 lines
1.3 KiB
Go
64 lines
1.3 KiB
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
|
||
}
|