From 426b1d61e5b3e5cbe561d2318fff1d6d8925a710 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 4 Sep 2026 00:43:07 +0000 Subject: [PATCH] Broadcast the control loop's own health with STATUS Whether the loop is keeping up is a question a display asks continuously, and answering it through the LOOP_STATS query means polling for something the controller already knows every tick. The period tail and the deadline-miss count now ride the status broadcast instead, appended at the tail so a decoder that stops at the fields it knows is unaffected. The status cache re-encodes on change, and these two change slowly: the percentile is recomputed once per stats window and overruns are rare, so the payload turns over about as often as the window does rather than every tick. StatusBuffer also declares drive_health, which PAROL6 never fills: the drivers report per-joint error FLAGS over the serial link, not analog temperature or current registers, and flags are a fault surface rather than a trend. An empty dict is the honest answer, and it is what tells a consumer "this backend has no such sensor" rather than "all zero". Co-Authored-By: Claude Opus 5 Claude-Session: https://claude.ai/code/session_01GdLL4oE6RejS9yPeSkQXpF --- parol6/protocol/wire.py | 25 +++++++++- parol6/server/status_cache.py | 16 +++++++ .../integration/test_loop_health_broadcast.py | 47 +++++++++++++++++++ 3 files changed, 86 insertions(+), 2 deletions(-) create mode 100644 tests/integration/test_loop_health_broadcast.py 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