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