76 lines
1.8 KiB
Go
76 lines
1.8 KiB
Go
package message
|
||
|
||
import (
|
||
"encoding/binary"
|
||
"fmt"
|
||
"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}
|
||
//hx := fmt.Sprintf("%X", b)
|
||
rotarySpeed := t.Speed * 60 / (float32(math.Pi) * float32(t.WheelDiameter) / 1000)
|
||
//rotarySpeed := t.Speed / (float32(math.Pi) * float32(t.WheelDiameter) / 1000) * 60
|
||
var rotarySpeedData uint32
|
||
if t.IsBack {
|
||
//b = binary.BigEndian.AppendUint32(b, uint32(0-rotarySpeed))
|
||
rotarySpeedData = uint32(0 - rotarySpeed)
|
||
} else {
|
||
//b = binary.BigEndian.AppendUint32(b, uint32(rotarySpeed))
|
||
rotarySpeedData = uint32(rotarySpeed)
|
||
}
|
||
b = binary.BigEndian.AppendUint32(b, rotarySpeedData)
|
||
crc := chkcrc(b) // crc校验码
|
||
b = binary.BigEndian.AppendUint16(b, crc)
|
||
//fmt.Println("发送:", hx, rotarySpeedData, crc)
|
||
return b
|
||
}
|
||
func (t *ElectricMachinery) Decode(d []byte) error {
|
||
if len(d) != 13 {
|
||
return fmt.Errorf("")
|
||
}
|
||
h := d[:7]
|
||
data := binary.BigEndian.Uint32(d[7:11])
|
||
crc := binary.BigEndian.Uint16(d[11:13])
|
||
hx := fmt.Sprintf("%X", h)
|
||
fmt.Println("接收", hx, data, crc)
|
||
return nil
|
||
}
|
||
|
||
// 生成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
|
||
}
|