feat: add motor power derating and thermal cutoff
This commit is contained in:
parent
c9005baf9a
commit
83312c277c
@ -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
|
||||
|
||||
19
README.md
19
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`。
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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("<b", frame.data, 1)[0]
|
||||
power_raw, speed_dps, encoder_raw = struct.unpack_from("<hhH", frame.data, 2)
|
||||
return MotorStatus2(
|
||||
motor_id=motor_id,
|
||||
temperature_c=temperature_c,
|
||||
power_raw=power_raw,
|
||||
speed_dps=speed_dps,
|
||||
encoder_raw=encoder_raw,
|
||||
)
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import threading
|
||||
import time
|
||||
from typing import Protocol
|
||||
|
||||
from .config import Settings
|
||||
@ -19,6 +20,8 @@ class CanTransport(Protocol):
|
||||
|
||||
def send(self, frame: CanFrame, *, tx_queue_timeout_s: float | None = None) -> 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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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()
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user