diff --git a/parol6/protocol/wire.py b/parol6/protocol/wire.py index 772e882..8a821f7 100644 --- a/parol6/protocol/wire.py +++ b/parol6/protocol/wire.py @@ -9,7 +9,7 @@ Wire format uses msgpack arrays with integer type codes: - OK: MsgType.OK (just the integer) - ERROR: [MsgType.ERROR, message] -- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed] +- STATUS: [MsgType.STATUS, pose, angles, speeds, io, action_current, action_state, joint_en, cart_en_wrf, cart_en_trf, executing_index, completed_index, last_checkpoint, error, queued_segments, queued_duration, action_params, tool_status, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, accepted_index, homed, enabled, homing_step, joints_homed, loop_health] - RESPONSE: [MsgType.RESPONSE, query_type, value] - COMMAND: [CmdType.XXX, ...params] """ @@ -1362,6 +1362,8 @@ def pack_status( enabled: bool = True, homing_step: int = 0, joints_homed: Sequence[int] = _NO_JOINTS_HOMED, + p99_period_s: float = 0.0, + overruns: int = 0, ) -> bytes: """Pack a status broadcast message. @@ -1409,6 +1411,7 @@ def pack_status( enabled, homing_step, joints_homed, + (p99_period_s, overruns), ), option=ormsgpack.OPT_SERIALIZE_NUMPY, ) @@ -1467,6 +1470,15 @@ class StatusBuffer: ) warnings: list[tuple] = field(default_factory=list) link_health: dict = field(default_factory=dict) + # The drives report per-joint error FLAGS over the serial link, not + # analog temperature or current registers, so there is nothing to put + # here: empty is what tells a consumer "no such sensor" rather than + # "all zero". Flags surface as faults, not as a trend. + drive_health: dict = field(default_factory=dict) + # The control loop's own health: p99_period_s and overruns. Empty from + # producers that predate the field, which is how a consumer tells + # "loop healthy" from "loop not reported". + loop_health: dict = field(default_factory=dict) # Firmware referencing progress in waldoctl's shape: active, sequence_step, # and one (HomingJointState, HomingPhase) pair per joint; empty while idle. homing: dict = field(default_factory=dict) @@ -1535,6 +1547,8 @@ def copy(self) -> "StatusBuffer": torques_ext=self.torques_ext.copy(), warnings=list(self.warnings), link_health=dict(self.link_health), + drive_health=dict(self.drive_health), + loop_health=dict(self.loop_health), homing=dict(self.homing), ) @@ -1581,7 +1595,8 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: error, queued_segments, queued_duration, action_params, tool_status_tuple, tcp_speed, simulator_active, collision_active, collision_pairs, scene_epoch, - accepted_index, homed, enabled, homing_step, joints_homed] + accepted_index, homed, enabled, homing_step, joints_homed, + loop_health] Args: data: Raw msgpack bytes @@ -1656,6 +1671,12 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool: buf.enabled = bool(msg[25]) if len(msg) > 25 else True if len(msg) > 27: _apply_homing_progress(buf, int(msg[26]), msg[27]) + if len(msg) > 28: + lh = msg[28] + buf.loop_health = { + "p99_period_s": float(lh[0]), + "overruns": int(lh[1]), + } return True except Exception as e: diff --git a/parol6/server/status_cache.py b/parol6/server/status_cache.py index b3d6278..9cb270b 100644 --- a/parol6/server/status_cache.py +++ b/parol6/server/status_cache.py @@ -166,6 +166,8 @@ def __init__(self) -> None: # All-joints-homed tracking field self._homed: bool = False + self._p99_period_s: float = 0.0 + self._overruns: int = 0 self._enabled: bool = True self._homing_step: int = 0 self._joints_homed: list[int] = [0] * 6 @@ -550,6 +552,17 @@ def update_from_state(self, state: ControllerState) -> None: self._queued_segments = state.queued_segments self._queued_duration = state.queued_duration + # The percentile is recomputed once per stats window and overruns + # are rare, so this re-encodes the cached payload about as often as + # the window turns rather than on every tick. + loop_changed = ( + self._p99_period_s != state.p99_period_s + or self._overruns != state.overrun_count + ) + if loop_changed: + self._p99_period_s = state.p99_period_s + self._overruns = state.overrun_count + # Mark binary cache dirty if anything changed if ( pos_changed @@ -566,6 +579,7 @@ def update_from_state(self, state: ControllerState) -> None: or homing_changed or collision_changed or depth_changed + or loop_changed ): self._binary_dirty = True @@ -602,6 +616,8 @@ def to_binary(self) -> bytes: enabled=self._enabled, homing_step=self._homing_step, joints_homed=self._joints_homed, + p99_period_s=self._p99_period_s, + overruns=self._overruns, ) self._binary_dirty = False return self._binary_cache diff --git a/tests/integration/test_loop_health_broadcast.py b/tests/integration/test_loop_health_broadcast.py new file mode 100644 index 0000000..5e3230e --- /dev/null +++ b/tests/integration/test_loop_health_broadcast.py @@ -0,0 +1,47 @@ +"""The control loop's own health rides the status broadcast. + +A display that wants to say whether the loop is keeping up should not have +to poll ``loop_stats()`` for it: the period tail and the deadline-miss +count are on every STATUS frame, and they are the live numbers rather than +a boot-time snapshot. +""" + +import asyncio + +import pytest + +from parol6 import AsyncRobotClient + + +@pytest.mark.asyncio +@pytest.mark.integration +async def test_status_carries_the_loops_own_health(server_proc, ports): + """STATUS reports the loop percentile and overrun count, and they agree + with what the LOOP_STATS query answers about the same loop.""" + async with AsyncRobotClient(port=ports.server_port) as client: + assert await client.wait_ready(timeout=10.0) + + seen: dict = {} + + async def collect() -> None: + async for status in client.stream_status_shared(): + health = dict(getattr(status, "loop_health", {}) or {}) + # The percentile needs a full sampling window before it + # means anything, so wait for it rather than taking + # whichever frame arrives first. + if health.get("p99_period_s", 0.0) > 0.0: + seen.update(health) + return + + try: + await asyncio.wait_for(collect(), timeout=15.0) + except asyncio.TimeoutError: + pytest.fail("no loop health ever arrived on STATUS") + + stats = await client.loop_stats() + assert stats is not None + assert abs(seen["p99_period_s"] - stats.p99_period_s) < 5e-3, ( + f"STATUS says p99 {seen['p99_period_s']}, " + f"the query says {stats.p99_period_s}" + ) + assert seen["overruns"] == stats.overrun_count