1"""Dependency-free teaching implementation of the Hall sensor wire format.23This is a new example, separate from firmware/host/bus.py. It implements4read-only requests and parsing; it is not a hardware-qualified robot driver.5"""67from __future__ import annotations89from dataclasses import dataclass10import math11import struct12import time1314SOF = b"\xa5\x5a"15HOST = 0x0016TYPE_ENCODER = 0x0117TYPE_POSITION = 0x0218TYPE_ANY = 0xFF19CMD_GET_INFO = 0x0120CMD_GET_DATA = 0x1021CMD_REPORT = 0x3022CMD_RESPONSE = 0x8023PAYLOAD_MAX = 3224BAUD_TABLE = (9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600)252627class ProtocolError(ValueError):28 """A valid frame carries a status or payload we cannot use."""293031def crc16(data: bytes) -> int:32 """CRC-16/MODBUS: init FFFF, reflected polynomial A001, no final xor."""33 crc = 0xFFFF34 for byte in data:35 crc ^= byte36 for _ in range(8):37 crc = (crc >> 1) ^ (0xA001 if crc & 1 else 0)38 return crc394041def build_frame(dst: int, src: int, dev_type: int, cmd: int, seq: int,42 payload: bytes = b"") -> bytes:43 if len(payload) > PAYLOAD_MAX:44 raise ValueError("payload exceeds the firmware limit of 32 bytes")45 body = bytes((dst, src, dev_type, cmd, seq, len(payload))) + payload46 return SOF + body + struct.pack("<H", crc16(body))474849@dataclass(frozen=True)50class Frame:51 dst: int52 src: int53 dev_type: int54 cmd: int55 seq: int56 payload: bytes575859class Parser:60 """Parse split or concatenated frames, resynchronizing after corrupt bytes."""6162 def __init__(self, gap_timeout_s: float = 0.02) -> None:63 self.buffer = bytearray()64 self.crc_errors = 065 self.length_errors = 066 self.partial_timeouts = 067 self.gap_timeout_s = gap_timeout_s68 self._last_data_at: float | None = None6970 def feed(self, data: bytes, *, now: float | None = None) -> list[Frame]:71 now = time.monotonic() if now is None else now72 # A truncated frame with an in-range LEN must not hide a complete later73 # frame forever. Only abandon it after an idle read, never merely on a74 # read boundary: arbitrary USB/serial chunk boundaries are normal.75 if (not data and self.buffer and self._last_data_at is not None and76 now - self._last_data_at >= self.gap_timeout_s):77 del self.buffer[0]78 self.partial_timeouts += 179 if data:80 self._last_data_at = now81 self.buffer.extend(data)82 frames = []83 while True:84 start = self.buffer.find(SOF)85 if start < 0:86 # A trailing A5 can be the first byte of the next call's SOF.87 self.buffer[:] = b"\xa5" if self.buffer[-1:] == b"\xa5" else b""88 break89 if start:90 del self.buffer[:start]91 if len(self.buffer) < 8:92 break93 length = self.buffer[7]94 if length > PAYLOAD_MAX:95 self.length_errors += 196 del self.buffer[0]97 continue98 total = 10 + length99 if len(self.buffer) < total:100 break101 body = bytes(self.buffer[2:8 + length])102 received = struct.unpack_from("<H", self.buffer, 8 + length)[0]103 if crc16(body) != received:104 self.crc_errors += 1105 del self.buffer[0]106 continue107 frames.append(Frame(*self.buffer[2:7], bytes(self.buffer[8:8 + length])))108 del self.buffer[:total]109 return frames110111112def checked_payload(payload: bytes, expected: int) -> bytes:113 if not payload:114 raise ProtocolError("response has no status byte")115 if payload[0] != 0:116 raise ProtocolError(f"device status 0x{payload[0]:02x}")117 if len(payload) != expected:118 raise ProtocolError(f"expected {expected} payload bytes, got {len(payload)}")119 return payload[1:]120121122def parse_info(payload: bytes) -> dict:123 data = checked_payload(payload, 13)124 dev_type, dev_id, mode, baud_index = data[:4]125 if dev_type not in (TYPE_ENCODER, TYPE_POSITION):126 raise ProtocolError(f"unknown measurement profile 0x{dev_type:02x}")127 report_ms, fw = struct.unpack_from("<HH", data, 4)128 zero = struct.unpack_from("<h" if dev_type == TYPE_POSITION else "<H", data, 10)[0]129 if not 1 <= dev_id <= 254:130 raise ProtocolError("device information contains an invalid address")131 if mode not in (0, 1) or baud_index >= len(BAUD_TABLE) or not 5 <= report_ms <= 60000:132 raise ProtocolError("device information contains invalid mode, baud or period")133 if data[8] not in (0, 1) or bool(data[8]) != bool(data[9] & 2):134 raise ProtocolError("calibration-valid byte and flag disagree")135 validate_flags(data[9], dev_type)136 if dev_type == TYPE_ENCODER and zero >= 36000:137 raise ProtocolError("angle zero lies outside one revolution")138 return {"dev_type": dev_type, "dev_id": dev_id, "mode": mode,139 "baud_index": baud_index, "report_ms": report_ms, "fw": fw,140 "cal_valid": data[8], "flags": data[9], "zero_raw": zero}141142143@dataclass(frozen=True)144class Sample:145 dev_type: int146 raw_value: int147 amplitude: int148 adc: tuple[int, int, int]149 flags: int150 host_rx_unix_s: float151 host_rx_monotonic_s: float152153 @property154 def si_value(self) -> float:155 if self.dev_type == TYPE_ENCODER:156 return self.raw_value * math.pi / 18000.0157 if self.dev_type == TYPE_POSITION:158 return self.raw_value * 0.00001159 raise ProtocolError("measurement profile is not recognized")160161 def to_dict(self) -> dict:162 encoder = self.dev_type == TYPE_ENCODER163 names = [name for bit, name in ((1, "weak_field"), (2, "calibrated"),164 (4, "calibrating"), (8, "out_of_array"))165 if self.flags & bit]166 warnings = [name for bit, name in ((1, "weak_field"), (4, "calibrating"),167 (8, "outside_array_or_interpolation"))168 if self.flags & bit]169 if not self.flags & 2:170 warnings.append("calibration_not_recorded")171 return {"profile": "encoder" if encoder else "line",172 "raw_value": self.raw_value,173 "raw_unit": "0.01_deg" if encoder else "0.01_mm",174 "display_value": self.raw_value / 100.0,175 "display_unit": "deg" if encoder else "mm",176 "si_value": self.si_value, "si_unit": "rad" if encoder else "m",177 "amplitude_adc_counts": self.amplitude,178 "adc_u1_u2_u4": list(self.adc), "flags": self.flags,179 "flag_names": names,180 "calibration_valid_flag": bool(self.flags & 2),181 "measurement_warnings": warnings,182 "host_rx_unix_s": self.host_rx_unix_s,183 "host_rx_monotonic_s": self.host_rx_monotonic_s,184 "timestamp_kind": "host_receive_decode_time_not_hardware_sample_time"}185186187def validate_flags(flags: int, dev_type: int) -> None:188 allowed = 0x07 if dev_type == TYPE_ENCODER else 0x0F189 if flags & ~allowed:190 raise ProtocolError("sample contains unsupported flags for this profile")191192193def parse_sample(payload: bytes, dev_type: int, *, with_status: bool = True,194 host_rx_unix_s: float | None = None,195 host_rx_monotonic_s: float | None = None) -> Sample:196 if dev_type not in (TYPE_ENCODER, TYPE_POSITION):197 raise ProtocolError("an explicit encoder or line profile is required")198 if with_status:199 payload = checked_payload(payload, 14)200 elif len(payload) != 13:201 raise ProtocolError("a report sample must contain exactly 13 bytes")202 value, amplitude, u1, u2, u4, flags = struct.unpack("<ihHHHB", payload)203 validate_flags(flags, dev_type)204 if amplitude < 0 or max(u1, u2, u4) > 4095:205 raise ProtocolError("sample contains a negative amplitude or non-12-bit ADC code")206 if dev_type == TYPE_ENCODER and not 0 <= value < 36000:207 raise ProtocolError("angle sample lies outside one revolution")208 # Do not clamp line values to +/-15 mm here: firmware subtracts the saved209 # signed mechanical zero after limiting its internal position estimate.210 return Sample(dev_type, value, amplitude, (u1, u2, u4), flags,211 time.time() if host_rx_unix_s is None else host_rx_unix_s,212 time.monotonic() if host_rx_monotonic_s is None else host_rx_monotonic_s)
元驭机器人Hall 传感器教学下载原文件