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

package message

import (
	"fmt"
	"math"
)

const (
	accHeader   = 0xA6
	uaidDefault = 0x71
	G           = 9.80665
)

type Accelerometer struct {
	Acc float32
	Aux byte
}

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 := 16; 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)

	//accCode := v << 8            //数据向左移动8位,取后3个字节
	//d2 = byte(accCode >> 16)     //取出d2数据
	//d1 = byte(accCode >> 8 << 8) // & 0xff //取出d1
	//d0 = byte(accCode << 20)     //& 0xf //取出d0高2位

	//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 (acc *Accelerometer) BuildCanData() []byte {
	accData := acc.Encode()
	appData := make([]byte, 0)
	appData = append(appData, accData...)
	appData = append(appData, byte(0))
	canId := CanFrameId{ID1: 0xff, ID2: 0xff, ID3: 0xff, ID4: 0xff}
	canNet := &CanetFrame{FF: false, RTR: false, CanLen: 7, CanId: canId, CanData: appData}
	return canNet.Encode()
}
func (acc *Accelerometer) Encode() []byte {
	d2, d1, d0 := encodeAcc(acc.Acc)
	if acc.Acc > 0 {
		setBit(d2, 7, 0)
	} else {
		setBit(d2, 7, 1)
	}
	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, byte(0))
	cs := checkSum(data)
	data = append(data, cs)
	return data
}

func checkSum(data []byte) byte {
	var sum uint16 = 0

	for _, d := range data {
		sum += uint16(d)
	}
	highByte := byte(sum >> 8)      // 取高8位
	lowByte := byte(sum)            // 取低8位
	secondSum := highByte + lowByte // 再次累加

	//inverse := ^secondSum
	return ^secondSum
}

func (acc *Accelerometer) Decode(Packet []byte) error {
	if len(Packet) < 7 {
		return fmt.Errorf("")
	}
	decode2Acc(Packet[4], Packet[3], Packet[2])

	return nil
}