50 lines
905 B
Go
50 lines
905 B
Go
package message
|
|
|
|
import (
|
|
"bytes"
|
|
"encoding/binary"
|
|
"fmt"
|
|
)
|
|
|
|
const (
|
|
accHeader = 0xA6
|
|
uaidDefault = 0x1C
|
|
)
|
|
|
|
type Accelerometer struct {
|
|
Acc uint32
|
|
}
|
|
|
|
func (acc *Accelerometer) Encode() []byte {
|
|
data := make([]byte, 0)
|
|
data = append(data, accHeader)
|
|
data = append(data, uaidDefault)
|
|
acc.Acc = acc.Acc | 0x0000FF
|
|
data = binary.LittleEndian.AppendUint32(data, acc.Acc)
|
|
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(data []byte) error {
|
|
if len(data) < 7 {
|
|
return fmt.Errorf("")
|
|
}
|
|
buf := bytes.NewBuffer(data)
|
|
_, _ = buf.ReadByte()
|
|
_, _ = buf.ReadByte()
|
|
var dataInt uint32 = 0
|
|
binary.Read(buf, binary.LittleEndian, &dataInt)
|
|
//newData := math.Float32frombits(dataInt & 0xffffc000)
|
|
acc.Acc = dataInt & 0xffffc000
|
|
return nil
|
|
}
|