rts-sim-testing-service/third_party/message/accelerometer.go
2024-07-25 14:55:46 +08:00

114 lines
2.3 KiB
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
}