43 lines
1.4 KiB
Python
43 lines
1.4 KiB
Python
|
import math
|
|||
|
|
import struct
|
|||
|
|
from typing import Dict, List, Tuple, Optional
|
|||
|
|
from can_msg_builder import BROADCAST_MIX_CMD_ID, BROADCAST_TORQUE_CMD_ID
|
|||
|
|
|
|||
|
|
|
|||
|
|
def parse_single_turn_state(data8: bytes) -> Dict:
|
|||
|
|
"""
|
|||
|
|
解析电机状态2回复can帧
|
|||
|
|
"""
|
|||
|
|
assert len(data8)==8
|
|||
|
|
temperature = struct.unpack("<b", data8[1:2])[0]
|
|||
|
|
iq_or_power = struct.unpack("<h", data8[2:4])[0]
|
|||
|
|
speed_dps = struct.unpack("<h", data8[4:6])[0]
|
|||
|
|
encoder = struct.unpack("<H", data8[6:8])[0]
|
|||
|
|
return dict(
|
|||
|
|
temperature_C=temperature,
|
|||
|
|
iq_or_power_raw=iq_or_power,
|
|||
|
|
speed_dps=speed_dps,
|
|||
|
|
rad=encoder / 65535 * 2 * math.pi,
|
|||
|
|
)
|
|||
|
|
|
|||
|
|
def parse_multi_turn_state(data8: bytes) -> Dict:
|
|||
|
|
"""
|
|||
|
|
解析“读取多圈角度(0x92)”回复帧
|
|||
|
|
- DATA[0] = 0x92
|
|||
|
|
- DATA[1..7] = int64 motorAngle 的低 7 字节（little-endian）
|
|||
|
|
- 单位：0.01 deg / LSB
|
|||
|
|
"""
|
|||
|
|
assert len(data8) == 8
|
|||
|
|
# 取低 7 字节
|
|||
|
|
low7 = data8[1:8] # bytes length = 7
|
|||
|
|
# 符号扩展：看第7字节(低7字节中的最高字节)的最高位是否为1
|
|||
|
|
# 如果为1，说明负数，需要补 0xFF；否则补 0x00
|
|||
|
|
sign_ext = 0xFF if (low7[6] & 0x80) else 0x00
|
|||
|
|
raw8 = low7 + bytes([sign_ext]) # 补成 8 字节 little-endian 的 int64
|
|||
|
|
|
|||
|
|
motor_angle_raw = struct.unpack("<q", raw8)[0] # int64
|
|||
|
|
motor_angle_deg = math.radians(motor_angle_raw * 0.01)
|
|||
|
|
|
|||
|
|
return dict(
|
|||
|
|
rad=motor_angle_deg, # 单位：0.01deg/LSB
|
|||
|
|
)
|