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
|
||
) |