154 lines
6.4 KiB
Python
154 lines
6.4 KiB
Python
"""Bounded VESC serial reader.
|
|
|
|
Wire reference: vedderb/vesc_tool dc53c658cbb89a947246034f7a00149cf79abdfc,
|
|
packet.cpp, commands.cpp and datatypes.h. No arbitrary packet transmit API.
|
|
Config payloads remain opaque until their exact firmware schema is admitted.
|
|
"""
|
|
|
|
import binascii
|
|
import struct
|
|
|
|
READ_COMMANDS = frozenset({0, 4, 14, 17, 31, 62})
|
|
MAX_PACKET = 10000
|
|
|
|
|
|
def request(command):
|
|
if type(command) is not int or command not in READ_COMMANDS:
|
|
raise ValueError("Unsupported read command")
|
|
data = bytes([command])
|
|
return b"\x02\x01" + data + binascii.crc_hqx(data, 0).to_bytes(2, "big") + b"\x03"
|
|
|
|
|
|
TEST_LIMITS = {"min_current_a": 0.5, "max_current_a": 30, "min_duration_s": 0.5,
|
|
"max_duration_s": 30, "current_ramp_a_per_s": 2.0,
|
|
"continuous_current": True, "max_erpm": 6000, "max_duty": 0.25,
|
|
"stall_current_a": 5, "stall_timeout_s": 2.0}
|
|
|
|
# A separate action/capability keeps old clients from silently changing modes.
|
|
SPEED_LIMITS = {"min_erpm": 300, "max_erpm": 3000, "ramp_erpm_per_s": 600,
|
|
"startup_timeout_s": 15, "settle_s": 1, "speed_tolerance": 0.15,
|
|
"lost_speed_timeout_s": 2, "duration_basis": "measured_speed"}
|
|
|
|
|
|
def frame(data):
|
|
if not 1 <= len(data) <= 255: raise ValueError("Invalid bounded command size")
|
|
return bytes([2, len(data)]) + data + binascii.crc_hqx(data, 0).to_bytes(2, "big") + b"\x03"
|
|
|
|
|
|
def speed_packet(erpm):
|
|
if type(erpm) not in (int, float) or not 0 <= erpm <= SPEED_LIMITS["max_erpm"]:
|
|
raise ValueError("Invalid test speed")
|
|
return frame(bytes([8]) + struct.pack(">i", round(erpm)))
|
|
|
|
|
|
def hall_packet():
|
|
# FW 5.02 native FOC Hall sweep, fixed 5 A; no store and no CAN forwarding.
|
|
return frame(bytes([28]) + struct.pack(">i", 5000))
|
|
|
|
|
|
def current_packet(current_a):
|
|
if type(current_a) not in (int, float) or not TEST_LIMITS["min_current_a"] <= current_a <= TEST_LIMITS["max_current_a"]:
|
|
raise ValueError("Test current must be between 0.5 and 30 A")
|
|
data = b"\x06" + struct.pack(">i", round(current_a * 1000))
|
|
return bytes([2, len(data)]) + data + binascii.crc_hqx(data, 0).to_bytes(2, "big") + b"\x03"
|
|
|
|
|
|
def test_packet(action):
|
|
"""Only fixed volatile commands; no arbitrary current/lease/packet."""
|
|
data = {"current": b"\x06" + struct.pack(">i", 2000),
|
|
"release": b"\x06" + bytes(4),
|
|
"claim": b"\x3f\x00" + struct.pack(">i", 250)}[action]
|
|
return bytes([2, len(data)]) + data + binascii.crc_hqx(data, 0).to_bytes(2, "big") + b"\x03"
|
|
|
|
|
|
def ppm(packet):
|
|
if len(packet) != 9 or packet[0] != 31:
|
|
raise ValueError("Incomplete PPM reply")
|
|
level, pulse = struct.unpack(">ii", packet[1:])
|
|
if not -1100000 <= level <= 1100000 or not 0 <= pulse <= 3000000:
|
|
raise ValueError("Invalid PPM values")
|
|
return {"level": level / 1e6, "pulse_ms": pulse / 1e6}
|
|
|
|
|
|
class Decoder:
|
|
def __init__(self):
|
|
self.buffer = bytearray()
|
|
|
|
def feed(self, data):
|
|
if len(self.buffer) + len(data) > MAX_PACKET * 2 + 16:
|
|
self.buffer.clear()
|
|
raise ValueError("Serial buffer overflow")
|
|
self.buffer.extend(data)
|
|
packets = []
|
|
while self.buffer:
|
|
start = self.buffer[0]
|
|
if start not in (2, 3, 4):
|
|
del self.buffer[0]
|
|
continue
|
|
width = start - 1
|
|
if len(self.buffer) < width + 1:
|
|
break
|
|
size = int.from_bytes(self.buffer[1:width + 1], "big")
|
|
if not 1 <= size <= MAX_PACKET:
|
|
del self.buffer[0]
|
|
continue
|
|
end = width + 1 + size
|
|
if len(self.buffer) < end + 3:
|
|
break
|
|
payload = bytes(self.buffer[width + 1:end])
|
|
if self.buffer[end + 2] != 3 or int.from_bytes(self.buffer[end:end + 2], "big") != binascii.crc_hqx(payload, 0):
|
|
del self.buffer[0]
|
|
continue
|
|
del self.buffer[:end + 3]
|
|
packets.append(payload)
|
|
return packets
|
|
|
|
|
|
def firmware(packet):
|
|
if len(packet) < 4 or packet[0] != 0:
|
|
raise ValueError("Incomplete firmware reply")
|
|
end = packet.find(b"\0", 3, 132)
|
|
if end <= 3 or len(packet) < end + 13:
|
|
raise ValueError("Firmware has no complete hardware identity")
|
|
name = packet[3:end].decode("ascii")
|
|
if not all(32 <= ord(c) < 127 for c in name):
|
|
raise ValueError("Invalid hardware name")
|
|
uuid = packet[end + 1:end + 13]
|
|
if uuid in (bytes(12), b"\xff" * 12):
|
|
raise ValueError("Invalid hardware UUID")
|
|
optional = packet[end + 13:]
|
|
return {"major": packet[1], "minor": packet[2], "version": f"{packet[1]}.{packet[2]:02d}",
|
|
"hardware": name, "uuid": uuid.hex(),
|
|
"test_firmware": optional[1] if len(optional) > 1 else None,
|
|
"hardware_type": optional[2] if len(optional) > 2 else None,
|
|
"custom_configs": optional[3] if len(optional) > 3 else None}
|
|
|
|
|
|
def values(packet):
|
|
if len(packet) < 54 or packet[0] != 4:
|
|
raise ValueError("Incomplete telemetry reply")
|
|
fields = (("mos_temperature_c", "h", 10), ("motor_temperature_c", "h", 10),
|
|
("motor_current_a", "i", 100), ("input_current_a", "i", 100),
|
|
("id_current_a", "i", 100), ("iq_current_a", "i", 100),
|
|
("duty", "h", 1000), ("erpm", "i", 1), ("input_voltage_v", "h", 10),
|
|
("amp_hours", "i", 10000), ("amp_hours_charged", "i", 10000),
|
|
("watt_hours", "i", 10000), ("watt_hours_charged", "i", 10000),
|
|
("tachometer", "i", 1), ("tachometer_abs", "i", 1), ("fault_code", "B", 1))
|
|
result, offset = {}, 1
|
|
for key, fmt, scale in fields:
|
|
result[key] = struct.unpack_from(">" + fmt, packet, offset)[0] / scale
|
|
offset += struct.calcsize(fmt)
|
|
# Optional tail is ordered, not an independent set of guessed offsets.
|
|
for key, fmt, scale in (("position_deg", "i", 1e6), ("can_id", "B", 1),
|
|
("mos1_c", "h", 10), ("mos2_c", "h", 10), ("mos3_c", "h", 10),
|
|
("vd_v", "i", 1000), ("vq_v", "i", 1000), ("status", "B", 1)):
|
|
size = struct.calcsize(fmt)
|
|
if len(packet) < offset + size:
|
|
break
|
|
result[key] = struct.unpack_from(">" + fmt, packet, offset)[0] / scale
|
|
offset += size
|
|
if "status" in result:
|
|
flags = int(result.pop("status"))
|
|
result.update(timeout=bool(flags & 1), kill_switch=bool(flags & 2))
|
|
return result
|