57 lines
1.2 KiB
Go
57 lines
1.2 KiB
Go
package message
|
||
|
||
import (
|
||
"encoding/binary"
|
||
"math"
|
||
)
|
||
|
||
// 速度转转速计算公式：
|
||
// 转速=车速 ÷ 车轮周长
|
||
// 车的轮径为d，车速为v1，电机转速为v2
|
||
// 公式为：V2=V1÷（πd）×60
|
||
type ElectricMachinery struct {
|
||
Speed float32 // 车速,m/s
|
||
WheelDiameter int32 // 轮径,mm
|
||
IsBack bool // 是否倒车
|
||
}
|
||
|
||
func (t *ElectricMachinery) Encode() []byte {
|
||
b := []byte{0x01, 0x10, 0x60, 0xff, 0x00, 0x02, 0x04}
|
||
rotarySpeed := t.Speed * 60 / (float32(math.Pi) * float32(t.WheelDiameter) / 1000)
|
||
if t.IsBack {
|
||
b = binary.BigEndian.AppendUint32(b, uint32(0-rotarySpeed))
|
||
} else {
|
||
b = binary.BigEndian.AppendUint32(b, uint32(rotarySpeed))
|
||
}
|
||
crc := chkcrc(b) // crc校验码
|
||
b = binary.BigEndian.AppendUint16(b, crc)
|
||
return b
|
||
}
|
||
|
||
// 生成crc
|
||
func chkcrc(data []byte) uint16 {
|
||
var crc uint16 = 0xFFFF
|
||
for i := 0; i < len(data); i++ {
|
||
crc = calccrc(data[i], crc)
|
||
}
|
||
hi := crc % 256
|
||
lo := crc / 256
|
||
crc = (hi << 8) | lo
|
||
return crc
|
||
}
|
||
|
||
// 计算crc
|
||
func calccrc(crcbuf byte, crc uint16) uint16 {
|
||
crc = crc ^ uint16(crcbuf)
|
||
for i := 0; i < 8; i++ {
|
||
chk := crc & 1
|
||
crc = crc >> 1
|
||
crc = crc & 0x7fff
|
||
if chk == 1 {
|
||
crc = crc ^ 0xa001
|
||
}
|
||
crc = crc & 0xffff
|
||
}
|
||
return crc
|
||
}
|