rts-sim-testing-service/third_party/message/electrical_machinery.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)
	}
	//fmt.Println(rotarySpeed, rotarySpeedData)
	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
}