From 83312c277c027186f19ffe525ea07180f0ef9ba9 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 12 Aug 2026 11:20:28 +0800 Subject: [PATCH] feat: add motor power derating and thermal cutoff --- .env.example | 8 ++ README.md | 19 +++++ src/lingkong_motor_service/api.py | 1 + src/lingkong_motor_service/config.py | 38 +++++++++ src/lingkong_motor_service/controller.py | 98 +++++++++++++++++++++++- src/lingkong_motor_service/protocol.py | 43 +++++++++++ src/lingkong_motor_service/transport.py | 31 ++++++++ tests/fakes.py | 12 +++ tests/test_config.py | 33 ++++++++ tests/test_controller.py | 88 +++++++++++++++++++++ tests/test_protocol.py | 29 +++++++ tests/test_transport.py | 29 +++++++ 12 files changed, 428 insertions(+), 1 deletion(-) diff --git a/.env.example b/.env.example index dc89ee0..5f6a285 100644 --- a/.env.example +++ b/.env.example @@ -19,6 +19,14 @@ MOTOR_CLOSE_CONTROL_RAW=400 # HTTP open/close 后持续广播直到 stop；服务正常退出会自动 Stop+Disable。 MOTOR_BROADCAST_FREQUENCY_HZ=10 +# open/close 开始 5 秒后保持原方向，将两台电机的开环 raw 幅值降为 100。 +MOTOR_DERATE_AFTER_S=5 +MOTOR_DERATED_CONTROL_RAW=100 + +# 每 0.5 秒请求一次状态2；任一电机达到 80C 后两路归零并停止。 +MOTOR_TEMPERATURE_LIMIT_C=80 +MOTOR_TEMPERATURE_POLL_INTERVAL_S=0.5 + # 监听全部 IPv4 网络接口；客户端应使用本机真实局域网 IP，不能使用 0.0.0.0。 MOTOR_HTTP_HOST=0.0.0.0 MOTOR_HTTP_PORT=8000 diff --git a/README.md b/README.md index f34112f..977de7b 100644 --- a/README.md +++ b/README.md @@ -12,6 +12,8 @@ - 持续广播频率：`10 Hz` - HTTP `open`：ID 1 = `+400` raw，ID 2 = `-400` raw - HTTP `close`：ID 1 = `-400` raw，ID 2 = `+400` raw +- `open`/`close` 持续 `5 s` 后：保持方向，两台电机幅值降为 `100` raw +- 温度达到 `80 C`：两台电机控制量归零并发送 Stop - 服务启动：自动 Enable；正常退出：自动 Stop 后 Disable - MS3008 开环 raw 范围：`-850..+850` @@ -123,6 +125,10 @@ MOTOR_IDS=1,2 MOTOR_OPEN_CONTROL_RAW=400 MOTOR_CLOSE_CONTROL_RAW=400 MOTOR_BROADCAST_FREQUENCY_HZ=10 +MOTOR_DERATE_AFTER_S=5 +MOTOR_DERATED_CONTROL_RAW=100 +MOTOR_TEMPERATURE_LIMIT_C=80 +MOTOR_TEMPERATURE_POLL_INTERVAL_S=0.5 MOTOR_HTTP_HOST=0.0.0.0 MOTOR_HTTP_PORT=8000 MOTOR_API_TOKEN= @@ -132,6 +138,15 @@ MOTOR_API_TOKEN= 固定方向：`open` 为 `{1: +400, 2: -400}`，`close` 为 `{1: -400, 2: +400}`。 +每次调用 `open` 或 `close` 都会重新开始计时。持续 5 秒后，程序保持当前方向并把 +两台电机的开环 raw 幅值降至 100，即 `open={1:+100,2:-100}`、 +`close={1:-100,2:+100}`。方向切换也会重新开始 5 秒计时。 + +运动期间服务每 0.5 秒发送一次 `0x9C` 状态2请求并读取温度。如果任一配置电机回报 +温度达到或超过 `80 C`，程序会把两台电机控制量都设为 0、发送 Stop，并锁存温度保护； +电机冷却后需要正常重启服务才能再次运动。保护依赖有效的状态2回包：如果 +`temperature_c_by_motor` 为空，表示尚未收到温度，不能认为温度保护已经得到现场验证。 + 完成接线和物理安全准备后，将 `MOTOR_HARDWARE_ENABLED` 改为 `true`。 此开关是启动安全总闸：服务启动后会立即自动 Enable，无需 HTTP 调用。 程序不会自动读取 `.env`，启动前导入： @@ -247,6 +262,10 @@ curl "http://${SERVER_IP}:8000/v1/motor/status" 实际响应还包含电机 ID、控制量、广播状态等字段。硬件未启用或状态冲突仍返回 HTTP `409`，CAN/USB 通讯失败仍返回 HTTP `503`。 +状态响应中的 `derated` 表示是否已进入 5 秒后的低功耗阶段， +`temperature_c_by_motor` 是各电机最近一次状态2温度，`temperature_limited` 表示 +80 C 归零保护是否已触发。 + `MOTOR_HARDWARE_ENABLED=true` 时，启动过程会自动 Enable。HTTP 只保留 `open`、`close`、`stop` 和 `status`，不提供 `enable`/`disable`。 diff --git a/src/lingkong_motor_service/api.py b/src/lingkong_motor_service/api.py index c002f23..ebc4149 100644 --- a/src/lingkong_motor_service/api.py +++ b/src/lingkong_motor_service/api.py @@ -117,6 +117,7 @@ def create_app( motor_status.hardware_enabled and motor_status.can_connected and motor_status.last_error is None + and not motor_status.temperature_limited ) return JSONResponse( status_code=status.HTTP_200_OK if ready else status.HTTP_503_SERVICE_UNAVAILABLE, diff --git a/src/lingkong_motor_service/config.py b/src/lingkong_motor_service/config.py index 7c4e89f..caa3c09 100644 --- a/src/lingkong_motor_service/config.py +++ b/src/lingkong_motor_service/config.py @@ -69,6 +69,10 @@ class Settings: open_control_raw: int = 400 close_control_raw: int = 400 broadcast_frequency_hz: float = 10.0 + derate_after_s: float = 5.0 + derated_control_raw: int = 100 + temperature_limit_c: int = 80 + temperature_poll_interval_s: float = 0.5 http_host: str = "0.0.0.0" http_port: int = 8000 api_token: str | None = None @@ -85,6 +89,10 @@ class Settings: open_control_raw=_env_int("MOTOR_OPEN_CONTROL_RAW", 400), close_control_raw=_env_int("MOTOR_CLOSE_CONTROL_RAW", 400), broadcast_frequency_hz=_env_float("MOTOR_BROADCAST_FREQUENCY_HZ", 10.0), + derate_after_s=_env_float("MOTOR_DERATE_AFTER_S", 5.0), + derated_control_raw=_env_int("MOTOR_DERATED_CONTROL_RAW", 100), + temperature_limit_c=_env_int("MOTOR_TEMPERATURE_LIMIT_C", 80), + temperature_poll_interval_s=_env_float("MOTOR_TEMPERATURE_POLL_INTERVAL_S", 0.5), http_host=os.getenv("MOTOR_HTTP_HOST", "0.0.0.0").strip(), http_port=_env_int("MOTOR_HTTP_PORT", 8000), api_token=token, @@ -135,6 +143,36 @@ class Settings: or not 1.0 <= self.broadcast_frequency_hz <= 1_000.0 ): raise ConfigError("MOTOR_BROADCAST_FREQUENCY_HZ must be in 1..1000") + if ( + isinstance(self.derate_after_s, bool) + or not isinstance(self.derate_after_s, int | float) + or not isfinite(self.derate_after_s) + or not 0.01 <= self.derate_after_s <= 3_600.0 + ): + raise ConfigError("MOTOR_DERATE_AFTER_S must be in 0.01..3600") + if ( + isinstance(self.derated_control_raw, bool) + or not isinstance(self.derated_control_raw, int) + or not 0 <= self.derated_control_raw <= MS_OPEN_LOOP_MAX + ): + raise ConfigError(f"MOTOR_DERATED_CONTROL_RAW must be in 0..{MS_OPEN_LOOP_MAX}") + if self.derated_control_raw > min(self.open_control_raw, self.close_control_raw): + raise ConfigError( + "MOTOR_DERATED_CONTROL_RAW must not exceed the open/close control values" + ) + if ( + isinstance(self.temperature_limit_c, bool) + or not isinstance(self.temperature_limit_c, int) + or not 1 <= self.temperature_limit_c <= 127 + ): + raise ConfigError("MOTOR_TEMPERATURE_LIMIT_C must be in 1..127") + if ( + isinstance(self.temperature_poll_interval_s, bool) + or not isinstance(self.temperature_poll_interval_s, int | float) + or not isfinite(self.temperature_poll_interval_s) + or not 0.05 <= self.temperature_poll_interval_s <= 60.0 + ): + raise ConfigError("MOTOR_TEMPERATURE_POLL_INTERVAL_S must be in 0.05..60") if not self.http_host: raise ConfigError("MOTOR_HTTP_HOST must not be empty") if not 1 <= self.http_port <= 65_535: diff --git a/src/lingkong_motor_service/controller.py b/src/lingkong_motor_service/controller.py index 51271fd..9883c03 100644 --- a/src/lingkong_motor_service/controller.py +++ b/src/lingkong_motor_service/controller.py @@ -14,6 +14,8 @@ from .protocol import ( MotorCommand, build_mixed_command_frame, build_open_loop_frame, + build_status_2_request, + parse_status_2_reply, ) from .transport import CanTransport, CanTransportError @@ -54,6 +56,12 @@ class MotorStatus: can_connected: bool broadcast_frequency_hz: float streaming: bool + derated: bool + derate_after_s: float + derated_control_raw: int + temperature_c_by_motor: dict[int, int] + temperature_limit_c: int + temperature_limited: bool last_error: str | None def to_dict(self) -> dict[str, Any]: @@ -62,6 +70,9 @@ class MotorStatus: result["control_raw_by_motor"] = { str(motor_id): value for motor_id, value in self.control_raw_by_motor.items() } + result["temperature_c_by_motor"] = { + str(motor_id): value for motor_id, value in self.temperature_c_by_motor.items() + } return result @@ -83,6 +94,10 @@ class MotorController: self._state = MotorState.DISABLED self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in settings.motor_ids} + self._movement_started_ns: int | None = None + self._derated = False + self._temperature_c_by_motor: dict[int, int] = {} + self._temperature_limited = False self._last_error: str | None = None self._broadcast_stop: threading.Event | None = None self._broadcast_thread: threading.Thread | None = None @@ -105,6 +120,12 @@ class MotorController: and self._broadcast_thread.is_alive() and self._state in {MotorState.OPENING, MotorState.CLOSING} ), + derated=self._derated, + derate_after_s=self._settings.derate_after_s, + derated_control_raw=self._settings.derated_control_raw, + temperature_c_by_motor=dict(self._temperature_c_by_motor), + temperature_limit_c=self._settings.temperature_limit_c, + temperature_limited=self._temperature_limited, last_error=self._last_error, ) @@ -125,6 +146,10 @@ class MotorController: self._state = MotorState.READY self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in self._settings.motor_ids} + self._movement_started_ns = None + self._derated = False + self._temperature_c_by_motor = {} + self._temperature_limited = False self._last_error = None time.sleep(0.02) return self.status() @@ -164,6 +189,8 @@ class MotorController: self._state = state self._control_raw_by_motor = dict(values) self._control_raw = values.get(1, next(iter(values.values()), 0)) + self._movement_started_ns = time.monotonic_ns() + self._derated = False self._last_error = None # Establish the background safety path before the first non-zero # frame. The controller lock keeps the new thread from sending @@ -176,6 +203,10 @@ class MotorController: self._require_hardware() if self._state is MotorState.FAULT: raise MotorCommunicationError(self._last_error or "motor controller is faulted") + if self._temperature_limited: + raise MotorNotEnabledError( + "temperature protection is latched; restart the service after the motors cool" + ) if not self._armed_by_service or self._state is MotorState.DISABLED: raise MotorNotEnabledError("motor auto-enable did not complete during service startup") @@ -205,6 +236,8 @@ class MotorController: self._state = MotorState.DISABLED self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in self._settings.motor_ids} + self._movement_started_ns = None + self._derated = False self._last_error = None return self.status() @@ -250,7 +283,12 @@ class MotorController: def _broadcast_loop(self, stop_event: threading.Event) -> None: period_ns = max(1, round(1_000_000_000 / self._settings.broadcast_frequency_hz)) + derate_after_ns = round(self._settings.derate_after_s * 1_000_000_000) + temperature_poll_period_ns = round( + self._settings.temperature_poll_interval_s * 1_000_000_000 + ) next_deadline_ns = time.monotonic_ns() + period_ns + next_temperature_poll_ns = time.monotonic_ns() try: while True: wait_s = max(0.0, (next_deadline_ns - time.monotonic_ns()) / 1_000_000_000) @@ -260,9 +298,36 @@ class MotorController: with self._lock: if self._state not in {MotorState.OPENING, MotorState.CLOSING}: return + now_ns = time.monotonic_ns() + if ( + not self._derated + and self._movement_started_ns is not None + and now_ns - self._movement_started_ns >= derate_after_ns + ): + self._control_raw_by_motor = { + motor_id: ( + self._settings.derated_control_raw + if value > 0 + else -self._settings.derated_control_raw + if value < 0 + else 0 + ) + for motor_id, value in self._control_raw_by_motor.items() + } + self._control_raw = self._control_raw_by_motor.get( + 1, next(iter(self._control_raw_by_motor.values()), 0) + ) + self._derated = True frame = build_open_loop_frame(self._control_raw_by_motor) try: + if now_ns >= next_temperature_poll_ns: + self._transport.send(build_status_2_request(self._settings.motor_ids)) + next_temperature_poll_ns = now_ns + temperature_poll_period_ns + self._read_temperature_replies() + with self._lock: + if self._temperature_limited: + return self._transport.send(frame) except CanTransportError as exc: with self._lock: @@ -286,6 +351,30 @@ class MotorController: self._broadcast_thread = None self._broadcast_stop = None + def _read_temperature_replies(self) -> None: + while True: + frame = self._transport.receive(timeout_s=0.0) + if frame is None: + return + report = parse_status_2_reply(frame) + if report is None or report.motor_id not in self._settings.motor_ids: + continue + with self._lock: + self._temperature_c_by_motor[report.motor_id] = report.temperature_c + if report.temperature_c >= self._settings.temperature_limit_c: + self._temperature_limited = True + self._state = MotorState.STOPPED + self._control_raw = 0 + self._control_raw_by_motor = { + motor_id: 0 for motor_id in self._settings.motor_ids + } + self._movement_started_ns = None + self._last_error = ( + f"motor {report.motor_id} temperature {report.temperature_c}C reached " + f"the {self._settings.temperature_limit_c}C limit; output set to zero" + ) + return + def _best_effort_sender_stop(self) -> None: frames = ( build_open_loop_frame({}), @@ -338,7 +427,10 @@ class MotorController: self._state = MotorState.STOPPED self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in self._settings.motor_ids} - self._last_error = None + self._movement_started_ns = None + self._derated = False + if not self._temperature_limited: + self._last_error = None def _best_effort_disable_locked(self) -> None: zero_frame = build_open_loop_frame({}) @@ -374,6 +466,8 @@ class MotorController: self._state = MotorState.FAULT if cleanup_error else MotorState.DISABLED self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in self._settings.motor_ids} + self._movement_started_ns = None + self._derated = False def _send_sequence_locked(self, *frames: CanFrame) -> None: for frame in frames: @@ -395,6 +489,8 @@ class MotorController: self._state = MotorState.FAULT self._control_raw = 0 self._control_raw_by_motor = {motor_id: 0 for motor_id in self._settings.motor_ids} + self._movement_started_ns = None + self._derated = False self._last_error = message def _require_hardware(self) -> None: diff --git a/src/lingkong_motor_service/protocol.py b/src/lingkong_motor_service/protocol.py index 889be0f..0f20939 100644 --- a/src/lingkong_motor_service/protocol.py +++ b/src/lingkong_motor_service/protocol.py @@ -13,6 +13,7 @@ MOTOR_ID_MIN = 1 MOTOR_ID_MAX = 4 MS_OPEN_LOOP_MIN = -850 MS_OPEN_LOOP_MAX = 850 +MOTOR_REPLY_BASE_ID = 0x140 class ProtocolError(ValueError): @@ -25,6 +26,7 @@ class MotorCommand(IntEnum): DISABLE = 0x80 STOP = 0x81 ENABLE = 0x88 + READ_STATUS_2 = 0x9C @dataclass(frozen=True, slots=True) @@ -40,6 +42,17 @@ class CanFrame: raise ProtocolError("V2.35 broadcast frames must have DLC=8") +@dataclass(frozen=True, slots=True) +class MotorStatus2: + """Decoded V2.35 status2 reply for one motor.""" + + motor_id: int + temperature_c: int + power_raw: int + speed_dps: int + encoder_raw: int + + def _validate_motor_id(motor_id: int) -> None: if ( isinstance(motor_id, bool) @@ -89,3 +102,33 @@ def build_mixed_command_frame(commands_by_motor: Mapping[int, MotorCommand]) -> arbitration_id=MIXED_COMMAND_FRAME_ID, data=bytes(payload), ) + + +def build_status_2_request(motor_ids: tuple[int, ...]) -> CanFrame: + """Build a mixed broadcast request for each configured motor's status2.""" + + return build_mixed_command_frame( + {motor_id: MotorCommand.READ_STATUS_2 for motor_id in motor_ids} + ) + + +def parse_status_2_reply(frame: CanFrame) -> MotorStatus2 | None: + """Decode a status2 reply, returning ``None`` for unrelated frames.""" + + motor_id = frame.arbitration_id - MOTOR_REPLY_BASE_ID + if ( + frame.is_extended_id + or not MOTOR_ID_MIN <= motor_id <= MOTOR_ID_MAX + or frame.data[0] != MotorCommand.READ_STATUS_2 + ): + return None + + temperature_c = struct.unpack_from(" None: ... + def receive(self, *, timeout_s: float = 0.0) -> CanFrame | None: ... + def shutdown(self) -> None: ... @@ -34,6 +37,10 @@ class DisabledCanTransport: del tx_queue_timeout_s raise CanTransportError("hardware access is disabled by MOTOR_HARDWARE_ENABLED") + def receive(self, *, timeout_s: float = 0.0) -> CanFrame | None: + del timeout_s + raise CanTransportError("hardware access is disabled by MOTOR_HARDWARE_ENABLED") + def shutdown(self) -> None: return None @@ -91,6 +98,30 @@ class PythonCanTransport: except Exception as exc: raise CanTransportError(f"CAN send failed: {exc}") from exc + def receive(self, *, timeout_s: float = 0.0) -> CanFrame | None: + deadline = time.monotonic() + max(0.0, timeout_s) + try: + with self._lock: + if not self._is_open: + raise CanTransportError("CAN transport is closed") + while True: + remaining_s = max(0.0, deadline - time.monotonic()) + message = self._bus.recv(timeout=remaining_s) + if message is None: + return None + if len(message.data) == 8: + return CanFrame( + arbitration_id=message.arbitration_id, + data=bytes(message.data), + is_extended_id=message.is_extended_id, + ) + if remaining_s == 0.0: + return None + except CanTransportError: + raise + except Exception as exc: + raise CanTransportError(f"CAN receive failed: {exc}") from exc + def shutdown(self) -> None: with self._lock: if not self._is_open: diff --git a/tests/fakes.py b/tests/fakes.py index 9584cb6..ed7633b 100644 --- a/tests/fakes.py +++ b/tests/fakes.py @@ -13,6 +13,7 @@ class FakeTransport: self.tx_queue_timeouts_s: list[float | None] = [] self.is_open = True self.fail_next = False + self.received_frames: list[CanFrame] = [] self.lock = threading.Lock() def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> None: @@ -26,6 +27,17 @@ class FakeTransport: self.frame_times_ns.append(time.monotonic_ns()) self.tx_queue_timeouts_s.append(tx_queue_timeout_s) + def receive(self, *, timeout_s: float = 0.0) -> CanFrame | None: + del timeout_s + with self.lock: + if not self.received_frames: + return None + return self.received_frames.pop(0) + + def queue_received(self, frame: CanFrame) -> None: + with self.lock: + self.received_frames.append(frame) + def shutdown(self) -> None: with self.lock: self.is_open = False diff --git a/tests/test_config.py b/tests/test_config.py index 9b8ba0a..1d4a504 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -13,6 +13,10 @@ def test_defaults_match_connected_ms3008_installation() -> None: assert settings.open_control_raw == 400 assert settings.close_control_raw == 400 assert settings.broadcast_frequency_hz == 10 + assert settings.derate_after_s == 5 + assert settings.derated_control_raw == 100 + assert settings.temperature_limit_c == 80 + assert settings.temperature_poll_interval_s == 0.5 assert settings.http_host == "0.0.0.0" assert settings.api_token is None @@ -38,6 +42,10 @@ def test_environment_values_are_parsed(monkeypatch: pytest.MonkeyPatch) -> None: monkeypatch.setenv("MOTOR_IDS", "2, 1") monkeypatch.setenv("MOTOR_OPEN_CONTROL_RAW", "123") monkeypatch.setenv("MOTOR_CLOSE_CONTROL_RAW", "234") + monkeypatch.setenv("MOTOR_DERATE_AFTER_S", "6") + monkeypatch.setenv("MOTOR_DERATED_CONTROL_RAW", "90") + monkeypatch.setenv("MOTOR_TEMPERATURE_LIMIT_C", "75") + monkeypatch.setenv("MOTOR_TEMPERATURE_POLL_INTERVAL_S", "0.25") settings = Settings.from_env() @@ -46,6 +54,10 @@ def test_environment_values_are_parsed(monkeypatch: pytest.MonkeyPatch) -> None: assert settings.motor_ids == (2, 1) assert settings.open_control_raw == 123 assert settings.close_control_raw == 234 + assert settings.derate_after_s == 6 + assert settings.derated_control_raw == 90 + assert settings.temperature_limit_c == 75 + assert settings.temperature_poll_interval_s == 0.25 @pytest.mark.parametrize("channel", [-1, 2, "CAN1", True]) @@ -75,3 +87,24 @@ def test_broadcast_frequency_must_be_finite_and_bounded(frequency: float) -> Non def test_external_http_binding_does_not_require_api_token() -> None: Settings(http_host="0.0.0.0", api_token=None).validate() + + +@pytest.mark.parametrize( + ("field", "value", "message"), + [ + ("derate_after_s", 0, "MOTOR_DERATE_AFTER_S"), + ("derated_control_raw", 851, "MOTOR_DERATED_CONTROL_RAW"), + ("temperature_limit_c", 128, "MOTOR_TEMPERATURE_LIMIT_C"), + ("temperature_poll_interval_s", 0, "MOTOR_TEMPERATURE_POLL_INTERVAL_S"), + ], +) +def test_power_and_temperature_settings_are_bounded( + field: str, value: object, message: str +) -> None: + with pytest.raises(ConfigError, match=message): + Settings(**{field: value}).validate() + + +def test_derated_value_cannot_increase_the_configured_output() -> None: + with pytest.raises(ConfigError, match="must not exceed"): + Settings(open_control_raw=50, derated_control_raw=100).validate() diff --git a/tests/test_controller.py b/tests/test_controller.py index 893d0f1..e918222 100644 --- a/tests/test_controller.py +++ b/tests/test_controller.py @@ -78,6 +78,13 @@ def wait_for_payload_count(transport: FakeTransport, payload: bytes, count: int) raise AssertionError(f"did not observe {count} copies of {payload.hex()}") +def status_2_reply(motor_id: int, temperature_c: int) -> CanFrame: + return CanFrame( + arbitration_id=0x140 + motor_id, + data=bytes([0x9C, temperature_c & 0xFF, 0, 0, 0, 0, 0, 0]), + ) + + def test_requires_enable_before_motion() -> None: controller = MotorController(enabled_settings(), FakeTransport()) @@ -218,6 +225,87 @@ def test_open_keeps_broadcasting_until_explicit_stop() -> None: controller.shutdown() +@pytest.mark.parametrize( + ("action", "initial_payload", "derated_payload", "expected_values"), + [ + ("open", "900170fe00000000", "64009cff00000000", {1: 100, 2: -100}), + ("close", "70fe900100000000", "9cff640000000000", {1: -100, 2: 100}), + ], +) +def test_motion_derates_after_configured_delay_without_changing_direction( + action: str, + initial_payload: str, + derated_payload: str, + expected_values: dict[int, int], +) -> None: + transport = FakeTransport() + settings = enabled_settings( + broadcast_frequency_hz=200, + derate_after_s=0.03, + temperature_poll_interval_s=1, + ) + controller = MotorController(settings, transport) + controller.enable() + + getattr(controller, action)() + wait_for_payload_count(transport, bytes.fromhex(initial_payload), 2) + wait_for_payload_count(transport, bytes.fromhex(derated_payload), 2) + + status = controller.status() + assert status.derated is True + assert status.control_raw_by_motor == expected_values + assert status.temperature_limited is False + controller.stop() + controller.shutdown() + + +def test_temperature_below_limit_is_reported_without_stopping() -> None: + transport = FakeTransport() + transport.queue_received(status_2_reply(1, 79)) + controller = MotorController(enabled_settings(temperature_poll_interval_s=0.05), transport) + controller.enable() + + controller.open() + deadline = time.monotonic() + 0.2 + while 1 not in controller.status().temperature_c_by_motor and time.monotonic() < deadline: + time.sleep(0.002) + + status = controller.status() + assert status.temperature_c_by_motor == {1: 79} + assert status.temperature_limited is False + assert status.state is MotorState.OPENING + assert status.streaming is True + controller.stop() + controller.shutdown() + + +def test_temperature_at_limit_sets_both_outputs_to_zero_and_latches() -> None: + transport = FakeTransport() + transport.queue_received(status_2_reply(2, 80)) + controller = MotorController(enabled_settings(temperature_poll_interval_s=0.05), transport) + controller.enable() + + controller.open() + deadline = time.monotonic() + 0.2 + while not controller.status().temperature_limited and time.monotonic() < deadline: + time.sleep(0.002) + + status = controller.status() + assert status.temperature_c_by_motor == {2: 80} + assert status.temperature_limited is True + assert status.control_raw_by_motor == {1: 0, 2: 0} + assert status.state is MotorState.STOPPED + assert status.streaming is False + assert "motor 2 temperature 80C" in (status.last_error or "") + assert frame_dump(transport)[-2:] == [ + (CONTROL_FRAME_ID, "0000000000000000"), + (MIXED_COMMAND_FRAME_ID, "8100810000000000"), + ] + with pytest.raises(MotorNotEnabledError, match="temperature protection is latched"): + controller.close() + controller.shutdown() + + def test_enable_sends_zero_and_enable() -> None: transport = FakeTransport() controller = MotorController(enabled_settings(), transport) diff --git a/tests/test_protocol.py b/tests/test_protocol.py index e48b4ae..243145c 100644 --- a/tests/test_protocol.py +++ b/tests/test_protocol.py @@ -3,11 +3,16 @@ import pytest from lingkong_motor_service.protocol import ( CONTROL_FRAME_ID, MIXED_COMMAND_FRAME_ID, + MOTOR_REPLY_BASE_ID, MS_OPEN_LOOP_MAX, + CanFrame, MotorCommand, + MotorStatus2, ProtocolError, build_mixed_command_frame, build_open_loop_frame, + build_status_2_request, + parse_status_2_reply, ) @@ -54,3 +59,27 @@ def test_mixed_enable_stop_and_disable_vectors() -> None: assert enable.data == bytes.fromhex("88 00 00 00 00 00 00 00") assert stop.data == bytes.fromhex("00 00 00 00 00 00 81 00") assert disable.data == bytes.fromhex("00 00 80 00 00 00 00 00") + + +def test_status_2_request_and_reply_temperature() -> None: + request = build_status_2_request((1, 2)) + reply = CanFrame( + arbitration_id=MOTOR_REPLY_BASE_ID + 2, + data=bytes.fromhex("9C 50 64 00 0A 00 34 12"), + ) + + assert request.arbitration_id == MIXED_COMMAND_FRAME_ID + assert request.data == bytes.fromhex("9C 00 9C 00 00 00 00 00") + assert parse_status_2_reply(reply) == MotorStatus2( + motor_id=2, + temperature_c=80, + power_raw=100, + speed_dps=10, + encoder_raw=0x1234, + ) + + +def test_status_2_parser_ignores_unrelated_frame() -> None: + frame = CanFrame(arbitration_id=CONTROL_FRAME_ID, data=bytes(8)) + + assert parse_status_2_reply(frame) is None diff --git a/tests/test_transport.py b/tests/test_transport.py index ee3a80c..883e087 100644 --- a/tests/test_transport.py +++ b/tests/test_transport.py @@ -15,6 +15,7 @@ class StubBus: self.messages: list[can.Message] = [] self.timeouts: list[float | None] = [] self.shutdown_count = 0 + self.received_messages: list[can.Message] = [] def send(self, message: can.Message, timeout: float | None = None) -> None: self.messages.append(message) @@ -23,6 +24,12 @@ class StubBus: def shutdown(self) -> None: self.shutdown_count += 1 + def recv(self, timeout: float | None = None) -> can.Message | None: + del timeout + if not self.received_messages: + return None + return self.received_messages.pop(0) + def test_canalystii_configuration_and_standard_frame(monkeypatch: pytest.MonkeyPatch) -> None: captured: dict[str, object] = {} @@ -92,3 +99,25 @@ def test_shutdown_is_idempotent_and_send_after_shutdown_fails( assert bus.shutdown_count == 1 with pytest.raises(CanTransportError, match="CAN transport is closed"): transport.send(build_open_loop_frame({1: 0})) + + +def test_canalystii_receives_standard_status_frame(monkeypatch: pytest.MonkeyPatch) -> None: + bus = StubBus() + bus.received_messages.append( + can.Message( + arbitration_id=0x141, + is_extended_id=False, + data=bytes.fromhex("9C 4F 64 00 0A 00 34 12"), + ) + ) + monkeypatch.setattr(can, "Bus", lambda **kwargs: bus) + transport = PythonCanTransport(Settings(hardware_enabled=True)) + + try: + frame = transport.receive(timeout_s=0.01) + + assert frame is not None + assert frame.arbitration_id == 0x141 + assert frame.data == bytes.fromhex("9C 4F 64 00 0A 00 34 12") + finally: + transport.shutdown()