Broadcast the control loop's own health with STATUS

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:

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

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