114 lines
2.3 KiB
Go
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, IsTrainPcSim: false}
|
|
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
|
|
}
|