1"""霍尔板 RS485 主机协议和串口线程。23帧格式与固件 inc/rs485_proto.h 一致：A5 5A、小端、CRC-16/MODBUS。4"""56from __future__ import annotations78import queue9import struct10import threading11import time12from dataclasses import dataclass1314import serial15from serial.tools import list_ports1617SOF = b"\xA5\x5A"18HOST = 0x0019TYPE_ENCODER = 0x0120TYPE_POSITION = 0x0221TYPE_ANY = 0xFF22PAYLOAD_MAX = 322324CMD_GET_INFO = 0x0125CMD_SET_ID = 0x0226CMD_SET_BAUD = 0x0327CMD_SET_MODE = 0x0428CMD_SAVE = 0x0529CMD_FACTORY = 0x0630CMD_SET_PROFILE = 0x0731CMD_GET_DATA = 0x1032CMD_SET_ZERO = 0x2133CMD_CLEAR_ZERO = 0x2234CMD_CAL_BEGIN = 0x2335CMD_CAL_END = 0x2436CMD_CAL_CLEAR = 0x2537CMD_GET_CAL = 0x2638CMD_SET_SIGMA = 0x2739CMD_REPORT = 0x3040CMD_RESPONSE = 0x804142MODE_POLL = 043MODE_REPORT = 14445BAUD_TABLE = (9600, 19200, 38400, 57600, 115200, 230400, 460800, 921600)4647STATUS_TEXT = {48 0x00: "成功",49 0x01: "长度不对",50 0x02: "参数超出范围",51 0x03: "当前状态不允许",52 0x04: "校准数据不够",53 0x05: "Flash 写入失败",54 0x06: "不认识的命令",55}565758def crc16(data: bytes) -> int:59 crc = 0xFFFF60 for byte in data:61 crc ^= byte62 for _ in range(8):63 if crc & 1:64 crc = (crc >> 1) ^ 0xA00165 else:66 crc >>= 167 crc &= 0xFFFF68 return crc697071def build_frame(dst: int, src: int, dev_type: int, cmd: int, seq: int, payload: bytes = b"") -> bytes:72 if len(payload) > PAYLOAD_MAX:73 raise ValueError("payload too long")74 body = bytes((dst & 0xFF, src & 0xFF, dev_type & 0xFF, cmd & 0xFF, seq & 0xFF, len(payload))) + payload75 crc = crc16(body)76 return SOF + body + bytes((crc & 0xFF, (crc >> 8) & 0xFF))777879@dataclass80class Frame:81 dst: int82 src: int83 dev_type: int84 cmd: int85 seq: int86 payload: bytes878889@dataclass90class Sample:91 t: float92 angle_cdeg: int93 amplitude: int94 raw: tuple[int, int, int]95 flags: int969798@dataclass99class DeviceInfo:100 dev_type: int101 dev_id: int102 mode: int103 baud_index: int104 report_ms: int105 fw: int106 cal_valid: int107 flags: int108 zero_cdeg: int109110111class Parser:112 """从字节流里切出完整帧，校验失败时丢掉一个字节再同步。"""113114 def __init__(self) -> None:115 self._buf = bytearray()116117 def reset(self) -> None:118 self._buf.clear()119120 def feed(self, data: bytes) -> list[Frame]:121 self._buf.extend(data)122 frames: list[Frame] = []123 while True:124 frame = self._pop()125 if frame is None:126 break127 frames.append(frame)128 return frames129130 def _pop(self) -> Frame | None:131 buf = self._buf132 while True:133 if len(buf) < 10:134 return None135 if buf[0] != 0xA5 or buf[1] != 0x5A:136 del buf[0]137 continue138 plen = buf[7]139 if plen > PAYLOAD_MAX:140 del buf[0]141 continue142 total = 10 + plen143 if len(buf) < total:144 return None145 body = bytes(buf[2:8 + plen])146 got = buf[8 + plen] | (buf[9 + plen] << 8)147 if crc16(body) != got:148 del buf[0]149 continue150 frame = Frame(buf[2], buf[3], buf[4], buf[5], buf[6], bytes(buf[8:8 + plen]))151 del buf[:total]152 return frame153154155def status_text(code: int) -> str:156 return STATUS_TEXT.get(code, f"状态 {code}")157158159def parse_sample(payload: bytes, with_status: bool) -> tuple[str, int | Sample]:160 if with_status:161 if len(payload) < 1:162 return "bad", 0163 if payload[0] != 0:164 return "err", payload[0]165 payload = payload[1:]166 if len(payload) < 13:167 return "bad", 0168 angle, amp, u1, u2, u4, flags = struct.unpack_from("<ihHHHB", payload)169 sample = Sample(time.perf_counter(), angle, amp, (u1, u2, u4), flags)170 return "ok", sample171172173def parse_info(payload: bytes) -> tuple[str, int | DeviceInfo]:174 if len(payload) < 1:175 return "bad", 0176 if payload[0] != 0:177 return "err", payload[0]178 if len(payload) < 13:179 return "bad", 0180 report_ms, fw = struct.unpack_from("<HH", payload, 5)181 if payload[1] == TYPE_POSITION:182 zero = struct.unpack_from("<h", payload, 11)[0]183 else:184 zero = struct.unpack_from("<H", payload, 11)[0]185 info = DeviceInfo(186 dev_type=payload[1],187 dev_id=payload[2],188 mode=payload[3],189 baud_index=payload[4],190 report_ms=report_ms,191 fw=fw,192 cal_valid=payload[9],193 flags=payload[10],194 zero_cdeg=zero,195 )196 return "ok", info197198199def list_serial_ports() -> list[tuple[str, str]]:200 found = []201 for port in list_ports.comports():202 label = port.device203 if port.description:204 label = f"{port.device} {port.description}"205 found.append((port.device, label))206 return found207208209class Bus:210 """独占串口的后台线程。界面只投递命令、取走采样和事件。"""211212 def __init__(self) -> None:213 self.samples: queue.Queue[Sample] = queue.Queue(maxsize=8000)214 self.events: queue.Queue[tuple] = queue.Queue(maxsize=200)215 self.dropped = 0216 self._cmds: queue.Queue[tuple] = queue.Queue()217 self._quit = threading.Event()218 self._thread = threading.Thread(target=self._run, name="hall-rs485", daemon=True)219 self._ser: serial.Serial | None = None220 self._parser = Parser()221 self._seq = 0222 self._dev_id = 1223 self._dev_type = TYPE_ANY224 self._device_kind = TYPE_ENCODER225 self._streaming = False226 self._stream_mode = "poll"227 self._gap_s = 0.01228 self._device_mode: int | None = None229 self._report_ms = 20230 self._last_timeout_log = 0.0231 self._inbox: list[Frame] = []232233 def start(self) -> None:234 self._thread.start()235236 def stop(self) -> None:237 self._quit.set()238 self._cmds.put(("nop",))239 self._thread.join(timeout=1.5)240 ser = self._ser241 self._ser = None242 if ser is not None and ser.is_open:243 ser.close()244245 def submit(self, *cmd: object) -> None:246 self._cmds.put(cmd)247248 def offer_sample(self, sample: Sample) -> None:249 try:250 self.samples.put_nowait(sample)251 except queue.Full:252 self.dropped += 1253254 def _emit(self, *event: object) -> None:255 try:256 self.events.put_nowait(event)257 except queue.Full:258 pass259260 def _run(self) -> None:261 while not self._quit.is_set():262 try:263 self._drain_cmds()264 if self._quit.is_set():265 break266 if self._ser is None:267 self._quit.wait(0.05)268 continue269 if self._streaming and self._stream_mode == "poll":270 self._poll_once()271 if self._gap_s > 0:272 self._quit.wait(self._gap_s)273 else:274 self._pump(0.05)275 except (serial.SerialException, OSError) as exc:276 self._fail_link(str(exc))277278 def _drain_cmds(self) -> None:279 while True:280 try:281 cmd = self._cmds.get_nowait()282 except queue.Empty:283 return284 self._exec(cmd)285286 def _exec(self, cmd: tuple) -> None:287 name = cmd[0]288 if name == "nop":289 return290 if name == "connect":291 self._connect(str(cmd[1]), int(cmd[2]), int(cmd[3]), int(cmd[4]))292 return293 if name == "disconnect":294 self._close()295 self._emit("connected", False)296 return297 if self._ser is None:298 self._emit("log", "还没有连接串口")299 return300 if name == "stream":301 self._set_stream(str(cmd[1]), int(cmd[2]))302 return303 if name == "info":304 self._read_info()305 return306 if name == "zero":307 self._checked("设零点", self._transact(CMD_SET_ZERO, b"", 0.4), self._after_zero)308 return309 if name == "clear_zero":310 self._checked("清除零点", self._transact(CMD_CLEAR_ZERO, b"", 0.4), None)311 return312 if name == "cal_begin":313 self._checked("开始校准", self._transact(CMD_CAL_BEGIN, b"", 0.3), None)314 return315 if name == "cal_end":316 self._checked("结束校准", self._transact(CMD_CAL_END, b"", 0.4), self._after_cal)317 return318 if name == "cal_clear":319 self._checked("清除校准", self._transact(CMD_CAL_CLEAR, b"", 0.4), None)320 return321 if name == "set_id":322 self._set_id(int(cmd[1]))323 return324 if name == "set_baud":325 self._set_baud(int(cmd[1]))326 return327 if name == "factory":328 self._factory()329 return330 if name == "set_profile":331 self._set_profile(int(cmd[1]))332 return333 if name == "set_sigma":334 self._set_sigma(int(cmd[1]))335 return336 if name == "set_mode":337 self._apply_mode(int(cmd[1]), int(cmd[2]), quiet=False)338339 def _connect(self, port: str, baud: int, dev_id: int, dev_type: int) -> None:340 self._close()341 self._dev_id = dev_id342 self._dev_type = dev_type343 self._streaming = False344 try:345 self._ser = serial.Serial(port, baudrate=baud, bytesize=8, parity="N", stopbits=1, timeout=0.02)346 except serial.SerialException as exc:347 self._emit("log", f"打开 {port} 失败：{exc}")348 self._emit("connected", False)349 return350 self._parser.reset()351 self._inbox.clear()352 self._emit("connected", True)353 self._emit("log", f"已连接 {port} {baud} 8N1，设备号 {dev_id}")354 self._read_info()355356 def _close(self) -> None:357 self._streaming = False358 ser = self._ser359 self._ser = None360 if ser is not None and ser.is_open:361 ser.close()362363 def _fail_link(self, message: str) -> None:364 self._emit("log", f"串口中断：{message}")365 self._close()366 self._emit("connected", False)367368 def _next_seq(self) -> int:369 self._seq = (self._seq + 1) & 0xFF370 return self._seq371372 def _transact(self, cmd: int, payload: bytes, timeout: float) -> Frame | None:373 ser = self._ser374 if ser is None:375 return None376 seq = self._next_seq()377 frame = build_frame(self._dev_id, HOST, self._dev_type, cmd, seq, payload)378 ser.reset_input_buffer()379 self._parser.reset()380 self._inbox.clear()381 ser.write(frame)382 ser.flush()383 deadline = time.perf_counter() + timeout384 expect = cmd | CMD_RESPONSE385 while time.perf_counter() < deadline:386 self._pump(min(0.02, max(0.0, deadline - time.perf_counter())))387 matched = None388 for item in self._inbox:389 if item.cmd == expect and item.seq == seq and item.src == self._dev_id:390 matched = item391 break392 self._inbox.clear()393 if matched is not None:394 return matched395 return None396397 def _pump(self, seconds: float) -> None:398 ser = self._ser399 if ser is None:400 return401 deadline = time.perf_counter() + seconds402 while time.perf_counter() < deadline:403 waiting = ser.in_waiting404 data = ser.read(waiting or 1)405 if not data:406 return407 for frame in self._parser.feed(data):408 self._on_frame(frame)409410 def _on_frame(self, frame: Frame) -> None:411 if frame.src != self._dev_id:412 return413 if frame.cmd == CMD_REPORT:414 kind, value = parse_sample(frame.payload, with_status=False)415 if kind == "ok" and isinstance(value, Sample):416 self.offer_sample(value)417 return418 if frame.cmd & CMD_RESPONSE:419 self._inbox.append(frame)420421 def _poll_once(self) -> None:422 frame = self._transact(CMD_GET_DATA, b"", 0.08)423 if frame is None:424 now = time.monotonic()425 if now - self._last_timeout_log > 1.0:426 self._last_timeout_log = now427 self._emit("log", "查询超时")428 return429 kind, value = parse_sample(frame.payload, with_status=True)430 if kind == "ok" and isinstance(value, Sample):431 self.offer_sample(value)432 elif kind == "err":433 self._emit("log", f"读采样失败：{status_text(int(value))}")434435 def _read_info(self) -> None:436 frame = self._transact(CMD_GET_INFO, b"", 0.3)437 if frame is None:438 self._emit("log", "没有读到设备信息，确认设备号、类型和波特率")439 return440 kind, value = parse_info(frame.payload)441 if kind != "ok" or not isinstance(value, DeviceInfo):442 self._emit("log", f"读信息失败：{status_text(int(value) if kind == 'err' else 0)}")443 return444 self._device_mode = value.mode445 self._report_ms = value.report_ms446 self._dev_id = value.dev_id447 self._device_kind = value.dev_type448 if value.dev_type in (TYPE_ENCODER, TYPE_POSITION):449 self._dev_type = value.dev_type450 self._emit("info", value)451 mode = "主动上报" if value.mode == MODE_REPORT else "主从"452 kind_name = "直线位置" if value.dev_type == TYPE_POSITION else "角度编码器"453 self._emit("log", f"设备 {value.dev_id} {kind_name} 固件 {value.fw >> 8}.{value.fw & 0xFF} {mode} {value.report_ms} ms")454 if value.dev_type == TYPE_POSITION:455 cal = self._transact(CMD_GET_CAL, b"", 0.3)456 if cal is not None and cal.payload and cal.payload[0] == 0:457 self._after_cal(cal)458459 def _set_stream(self, mode: str, period_ms: int) -> None:460 if mode == "off":461 self._streaming = False462 self._emit("streaming", False)463 self._emit("log", "停止采集")464 return465 period_ms = max(0, period_ms)466 if mode == "report":467 period_ms = max(5, period_ms)468 if not self._apply_mode(MODE_REPORT, period_ms, quiet=True):469 self._emit("streaming", False)470 return471 self._stream_mode = "report"472 self._streaming = True473 self._emit("streaming", True)474 self._emit("log", f"主动上报，周期 {period_ms} ms")475 return476 if self._device_mode == MODE_REPORT:477 if not self._apply_mode(MODE_POLL, max(period_ms, 5), quiet=True):478 self._emit("streaming", False)479 return480 self._stream_mode = "poll"481 self._gap_s = max(0.0, period_ms / 1000.0)482 self._streaming = True483 self._emit("streaming", True)484 gap = "尽快" if period_ms <= 0 else f"间隔 {period_ms} ms"485 self._emit("log", f"主从查询，{gap}")486487 def _apply_mode(self, mode: int, period_ms: int, quiet: bool) -> bool:488 period_ms = min(60000, max(5, period_ms))489 if self._device_mode == mode and self._report_ms == period_ms:490 return True491 payload = bytes((mode & 0xFF,)) + struct.pack("<H", period_ms)492 frame = self._transact(CMD_SET_MODE, payload, 0.4)493 if frame is None:494 self._emit("log", "设置上传模式超时")495 return False496 if not frame.payload or frame.payload[0] != 0:497 code = frame.payload[0] if frame.payload else 0498 self._emit("log", f"设置上传模式失败：{status_text(code)}")499 return False500 self._device_mode = mode501 self._report_ms = period_ms502 if not quiet:503 name = "主动上报" if mode == MODE_REPORT else "主从"504 self._emit("log", f"上传模式已改为{name}，周期 {period_ms} ms")505 self._emit("mode", mode, period_ms)506 return True507508 def _checked(self, title: str, frame: Frame | None, follow) -> None:509 if frame is None:510 self._emit("log", f"{title}超时")511 return512 if not frame.payload or frame.payload[0] != 0:513 code = frame.payload[0] if frame.payload else 0514 self._emit("log", f"{title}失败：{status_text(code)}")515 return516 self._emit("log", f"{title}完成")517 if follow is not None:518 follow(frame)519520 def _after_zero(self, frame: Frame) -> None:521 if len(frame.payload) >= 3:522 fmt = "<h" if self._device_kind == TYPE_POSITION else "<H"523 zero = struct.unpack_from(fmt, frame.payload, 1)[0]524 self._emit("zero", zero)525526 def _after_cal(self, frame: Frame) -> None:527 if len(frame.payload) < 13:528 return529 offsets = struct.unpack_from("<hhh", frame.payload, 1)530 gains = struct.unpack_from("<HHH", frame.payload, 7)531 self._emit("cal", offsets, gains, frame.payload[1])532533 def _set_profile(self, dev_type: int) -> None:534 if dev_type not in (TYPE_ENCODER, TYPE_POSITION):535 self._emit("log", "测量模式只能是角度或直线")536 return537 frame = self._transact(CMD_SET_PROFILE, bytes((dev_type,)), 0.4)538 if frame is None or not frame.payload or frame.payload[0] != 0:539 code = frame.payload[0] if frame and frame.payload else 0540 self._emit("log", f"切换测量模式失败：{status_text(code) if frame else '超时'}")541 return542 self._dev_type = dev_type543 name = "直线位置" if dev_type == TYPE_POSITION else "角度编码器"544 self._emit("log", f"已切换为{name}，原来的零点和校准已清除")545 self._read_info()546547 def _set_sigma(self, sigma_um: int) -> None:548 frame = self._transact(CMD_SET_SIGMA, struct.pack("<H", int(sigma_um) & 0xFFFF), 0.4)549 if frame is None or not frame.payload or frame.payload[0] != 0:550 code = frame.payload[0] if frame and frame.payload else 0551 self._emit("log", f"写敏感宽度失败：{status_text(code) if frame else '超时'}")552 return553 got = struct.unpack_from("<H", frame.payload, 1)[0] if len(frame.payload) >= 3 else sigma_um554 self._emit("sigma", got)555 self._emit("log", f"敏感宽度已写成 {got / 1000:.1f} mm")556557 def _set_id(self, new_id: int) -> None:558 if not 1 <= new_id <= 254:559 self._emit("log", "设备号要在 1 到 254")560 return561 frame = self._transact(CMD_SET_ID, bytes((new_id,)), 0.4)562 if frame is None or not frame.payload or frame.payload[0] != 0:563 code = frame.payload[0] if frame and frame.payload else 0564 self._emit("log", f"修改设备号失败：{status_text(code) if frame else '超时'}")565 return566 self._dev_id = frame.payload[1]567 self._emit("id", self._dev_id)568 self._emit("log", f"设备号已改为 {self._dev_id}")569570 def _set_baud(self, index: int) -> None:571 if not 0 <= index < len(BAUD_TABLE):572 return573 frame = self._transact(CMD_SET_BAUD, bytes((index,)), 0.4)574 if frame is None or not frame.payload or frame.payload[0] != 0:575 code = frame.payload[0] if frame and frame.payload else 0576 self._emit("log", f"修改波特率失败：{status_text(code) if frame else '超时'}")577 return578 baud = BAUD_TABLE[frame.payload[1]]579 port = self._ser.port if self._ser is not None else ""580 self._close()581 time.sleep(0.05)582 try:583 self._ser = serial.Serial(port, baudrate=baud, bytesize=8, parity="N", stopbits=1, timeout=0.02)584 except serial.SerialException as exc:585 self._emit("log", f"设备已改到 {baud}，本机重新打开失败：{exc}")586 self._emit("connected", False)587 return588 self._parser.reset()589 self._emit("baud", baud)590 self._emit("log", f"波特率已改为 {baud}")591 self._read_info()592593 def _factory(self) -> None:594 frame = self._transact(CMD_FACTORY, b"", 0.5)595 if frame is None or not frame.payload or frame.payload[0] != 0:596 code = frame.payload[0] if frame and frame.payload else 0597 self._emit("log", f"恢复出厂失败：{status_text(code) if frame else '超时'}")598 return599 new_id = frame.payload[1] if len(frame.payload) > 1 else 1600 index = frame.payload[2] if len(frame.payload) > 2 else 4601 baud = BAUD_TABLE[index] if 0 <= index < len(BAUD_TABLE) else 115200602 port = self._ser.port if self._ser is not None else ""603 current = self._ser.baudrate if self._ser is not None else baud604 self._dev_id = new_id605 self._device_mode = MODE_POLL606 self._report_ms = 20607 self._streaming = False608 if current != baud and port:609 self._close()610 time.sleep(0.05)611 try:612 self._ser = serial.Serial(port, baudrate=baud, bytesize=8, parity="N", stopbits=1, timeout=0.02)613 except serial.SerialException as exc:614 self._emit("log", f"设备已恢复出厂，本机重新打开失败：{exc}")615 self._emit("connected", False)616 return617 self._parser.reset()618 self._dev_type = TYPE_ANY619 self._emit("factory", new_id, baud)620 self._emit("streaming", False)621 self._emit("log", f"已恢复出厂：设备号 {new_id}，{baud}，主从，角度编码器")622 self._read_info()
元驭机器人Hall 传感器教学下载原文件