86 lines
2.5 KiB
Python
86 lines
2.5 KiB
Python
"""LowCmd 序列化 (PRO 格式 616B + Blowfish 加密).
|
||
|
||
PRO 格式跟 free-dog-sdk EDU 版的关键差异:
|
||
- 总长 616B (EDU 614B)
|
||
- CRC 偏移 612..616 (EDU 在 610..614)
|
||
- CRC 前有 2 字节 0x0000 固定填充 (位置 610..612)
|
||
- CRC 算法用裸 gen_crc, 不 XOR (EDU 用 encryptCrc XOR 0xedcab9de)
|
||
- bandWidth 字节序是 BE (EDU 用 LE)
|
||
- SN/version 全 0 (EDU 填实际值)
|
||
|
||
发送前必须 Blowfish 加密 (MCU 期待加密数据).
|
||
|
||
布局表 (616 字节明文):
|
||
offset bytes 字段
|
||
0..2 2 head 0xfeef
|
||
2..3 1 levelFlag 0xff
|
||
3..4 1 frameReserve
|
||
4..12 8 SN (全 0)
|
||
12..20 8 version (全 0)
|
||
20..22 2 bandWidth 0x3ac0 BE
|
||
22..562 540 motorCmd[20] × 27B
|
||
562..566 4 bms (全 0)
|
||
566..606 40 wirelessRemote (全 0)
|
||
606..610 4 reserve (全 0)
|
||
610..612 2 固定填充 0x0000
|
||
612..616 4 CRC = gen_crc(cmd[:612])
|
||
"""
|
||
import struct
|
||
from typing import Optional
|
||
from ..types.low_cmd import LowCmd
|
||
from ..types.motor import MotorCmd
|
||
from ..utils.common import gen_crc
|
||
from .blowfish import Blowfish
|
||
|
||
|
||
def build_low_cmd_plain(lowcmd: LowCmd) -> bytes:
|
||
"""构造 PRO 格式 616B 明文 LowCmd."""
|
||
cmd = bytearray(616)
|
||
|
||
# 0..2 head
|
||
cmd[0:2] = lowcmd.head
|
||
# 2..3 levelFlag
|
||
cmd[2] = lowcmd.levelFlag
|
||
# 3..4 frameReserve
|
||
cmd[3] = lowcmd.frameReserve
|
||
# 4..12 SN
|
||
cmd[4:12] = lowcmd.SN
|
||
# 12..20 version
|
||
cmd[12:20] = lowcmd.version
|
||
# 20..22 bandWidth BE
|
||
cmd[20:22] = struct.pack('>H', lowcmd.bandWidth)
|
||
# 22..562 motorCmd[20] × 27B
|
||
offset = 22
|
||
for motor in lowcmd.motorCmd:
|
||
motor_bytes = motor.to_bytes()
|
||
assert len(motor_bytes) == 27, f'motorCmd 序列化长度错: {len(motor_bytes)}'
|
||
cmd[offset:offset + 27] = motor_bytes
|
||
offset += 27
|
||
# 562..566 bms (默认 0)
|
||
# 566..606 wirelessRemote
|
||
cmd[566:606] = lowcmd.wirelessRemote
|
||
# 606..610 reserve
|
||
cmd[606:610] = lowcmd.reserve
|
||
# 610..612 固定填充 0x0000
|
||
# 612..616 CRC
|
||
cmd[612:616] = gen_crc(memoryview(cmd)[:612])
|
||
|
||
return bytes(cmd)
|
||
|
||
|
||
def build_low_cmd_encrypted(lowcmd: LowCmd, bf: Blowfish) -> bytes:
|
||
"""构造并加密 LowCmd (PRO 真实发包格式).
|
||
|
||
Returns:
|
||
616 字节 Blowfish ECB 密文, 直接 sendto MCU :8007.
|
||
"""
|
||
plain = build_low_cmd_plain(lowcmd)
|
||
return bf.encrypt_ecb(plain)
|
||
|
||
|
||
def make_damping_cmd() -> LowCmd:
|
||
"""快捷: 全 damping 的 LowCmd (常用于初始化/紧急停)."""
|
||
cmd = LowCmd()
|
||
cmd.all_damping()
|
||
return cmd
|