Report per-joint drive faults, and make the status rate settable

parol6/ack_policy.py

@@ -14,6 +14,7 @@
     CmdType.WRITE_IO,
     CmdType.SET_TCP_OFFSET,
     CmdType.SET_SHAPES,
+    CmdType.SET_STATUS_RATE,
 }
 
 # Query command types (use request/response, not ACK)
@@ -36,6 +37,7 @@
     CmdType.IS_SIMULATOR,
     CmdType.TCP_OFFSET,
     CmdType.SHAPES,
+    CmdType.STATUS_RATE,
 }
 
 # Streaming commands are fire-and-forget (no ACK needed)

parol6/client/async_client.py

@@ -17,7 +17,13 @@
 from waldoctl import RobotClient as _RobotClientABC, Shape, ShapeWorld, ToolStatus
 from msgspec.structs import asdict
 from waldoctl.shapes import shape_from_wire
-from waldoctl.status import ActionState, ActivityResult, LoopStatsResult, ToolResult
+from waldoctl.status import (
+    ActionState,
+    ActivityResult,
+    LoopStatsResult,
+    StatusRate,
+    ToolResult,
+)
 from waldoctl.tools import ToolSpec
 
 from .. import config as cfg
@@ -65,6 +71,9 @@
     ReachableCmd,
     ResetCmd,
     ResetLoopStatsCmd,
+    SetStatusRateCmd,
+    StatusRateCmd,
+    StatusRateResultStruct,
     ResetStateCmd,
     Response,
     StopCmd,
@@ -931,6 +940,29 @@ async def reset_loop_stats(self) -> int:
         """
         return await self._send(ResetLoopStatsCmd())
 
+    async def set_status_rate(self, hz: float) -> int:
+        """Set the rate the controller broadcasts status at.
+
+        Category: Configuration
+
+        Example:
+            rbt.set_status_rate(100)
+        """
+        return await self._send(SetStatusRateCmd(hz=float(hz)))
+
+    async def status_rate(self) -> StatusRate | None:
+        """Current broadcast rate and the control rate it divides.
+
+        Category: Query
+
+        Example:
+            rate = rbt.status_rate()
+        """
+        resp = await self._request(StatusRateCmd())
+        if not isinstance(resp, StatusRateResultStruct):
+            return None
+        return StatusRate(hz=resp.hz, control_hz=resp.control_hz)
+
     async def select_tool(self, tool_name: str, variant_key: str = "") -> int:
         """Set the active end-effector tool on the controller.
 

parol6/client/sync_client.py

@@ -14,7 +14,12 @@
 from waldoctl.sync_tools import SyncTool
 
 from waldoctl import PingResult, ToolStatus
-from waldoctl.status import ActivityResult, LoopStatsResult, ToolResult
+from waldoctl.status import (
+    ActivityResult,
+    LoopStatsResult,
+    StatusRate,
+    ToolResult,
+)
 
 from waldoctl.types import Axis, Frame
 from ..protocol.wire import (
@@ -319,6 +324,14 @@ def reset_loop_stats(self) -> int:
         """Reset control-loop min/max metrics and overrun count."""
         return _run(self._inner.reset_loop_stats())
 
+    def set_status_rate(self, hz: float) -> int:
+        """Set the rate the controller broadcasts status at."""
+        return _run(self._inner.set_status_rate(hz))
+
+    def status_rate(self) -> StatusRate | None:
+        """Current broadcast rate and the control rate it divides."""
+        return _run(self._inner.status_rate())
+
     def tools(self) -> ToolResult | None:
         """Current tool and available tools.
 

parol6/commands/query_commands.py

@@ -22,6 +22,8 @@
     JointSpeedsCmd,
     LoopStatsCmd,
     LoopStatsResultStruct,
+    StatusRateCmd,
+    StatusRateResultStruct,
     PingCmd,
     PingResultStruct,
     PoseCmd,
@@ -182,6 +184,24 @@ def compute(self, state: "ControllerState") -> bytes:
         )
 
 
+@register_command(CmdType.STATUS_RATE)
+class StatusRateCommand(QueryCommand[StatusRateCmd]):
+    """Return the broadcast rate and the control rate it divides."""
+
+    PARAMS_TYPE = StatusRateCmd
+    QUERY_TYPE = QueryType.STATUS_RATE
+
+    __slots__ = ()
+
+    def compute(self, state: "ControllerState") -> bytes:
+        return pack_response(
+            StatusRateResultStruct(
+                hz=state.status_rate_hz,
+                control_hz=1.0 / max(cfg.INTERVAL_S, 1e-9),
+            )
+        )
+
+
 @register_command(CmdType.PING)
 class PingCommand(QueryCommand[PingCmd]):
     """Respond to ping requests."""

parol6/commands/utility_commands.py

@@ -11,13 +11,17 @@
     MotionCommand,
     SystemCommand,
 )
+from parol6.config import CONTROL_RATE_HZ
 from parol6.protocol.wire import (
     CheckpointCmd,
     CmdType,
     DelayCmd,
     ResetLoopStatsCmd,
     ResetStateCmd,
+    SetStatusRateCmd,
 )
+from parol6.utils.error_catalog import make_error
+from parol6.utils.error_codes import ErrorCode
 from parol6.protocol.wire import CommandCode
 from parol6.server.command_registry import register_command
 from parol6.server.state import ControllerState
@@ -89,6 +93,46 @@ def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
         return ExecutionStatusCode.COMPLETED
 
 
+@register_command(CmdType.SET_STATUS_RATE)
+class SetStatusRateCommand(SystemCommand[SetStatusRateCmd]):
+    """Change the status broadcast rate for this session.
+
+    Status is emitted every Nth control tick, so a rate that does not divide
+    the control rate evenly cannot be served. It is refused rather than
+    rounded to a neighbour: a capture taken at a rate nobody asked for is
+    wrong in a way nothing reports.
+    """
+
+    PARAMS_TYPE = SetStatusRateCmd
+
+    __slots__ = ()
+
+    def execute_step(self, state: "ControllerState") -> ExecutionStatusCode:
+        hz = float(self.p.hz)
+        control = int(CONTROL_RATE_HZ)
+        # Ordered so the modulo only ever sees a finite, in-range, integral
+        # divisor: int(0.5) is 0 and int(nan) raises, and either would leave
+        # as a generic tick failure instead of the refusal that names the
+        # rates this controller can serve.
+        if not (1.0 <= hz <= control) or not hz.is_integer() or control % int(hz) != 0:
+            allowed = ", ".join(
+                str(control // n) for n in range(1, control + 1) if control % n == 0
+            )
+            self.fail(
+                make_error(
+                    ErrorCode.SYS_STATUS_RATE_INVALID,
+                    requested=hz,
+                    control=control,
+                    allowed=allowed,
+                )
+            )
+            return ExecutionStatusCode.FAILED
+        state.status_rate_hz = hz
+        logger.info("Status broadcast rate set to %g Hz", hz)
+        self.finish()
+        return ExecutionStatusCode.COMPLETED
+
+
 @register_command(CmdType.CHECKPOINT)
 class CheckpointCommand(MotionCommand[CheckpointCmd]):
     """Queue marker that sets state.last_checkpoint on execution.

parol6/config.py

@@ -88,13 +88,23 @@ def _trace(self, msg, *args, **kwargs):
 STATUS_RATE_HZ: float = float(os.getenv("PAROL6_STATUS_RATE_HZ", "50"))
 STATUS_STALE_S: float = float(os.getenv("PAROL6_STATUS_STALE_S", "0.5"))
 
+
+def status_broadcast_interval(hz: float) -> int:
+    """Control ticks between status broadcasts at *hz*.
+
+    Derived on demand rather than fixed at import: the rate is a session
+    knob (SET_STATUS_RATE), and a constant computed from the boot rate would
+    be a second answer to the question of how often status goes out.
+    """
+    return max(1, int(CONTROL_RATE_HZ) // int(hz))
+
+
 # Validate STATUS_RATE_HZ divides evenly into CONTROL_RATE_HZ for polling
 if int(CONTROL_RATE_HZ) % int(STATUS_RATE_HZ) != 0:
     raise ValueError(
         f"STATUS_RATE_HZ ({STATUS_RATE_HZ}) must divide evenly into "
         f"CONTROL_RATE_HZ ({CONTROL_RATE_HZ})"
     )
-STATUS_BROADCAST_INTERVAL: int = int(CONTROL_RATE_HZ) // int(STATUS_RATE_HZ)
 
 # Max ticks to hold MOVE at trajectory endpoint waiting for Position_in to converge.
 # At 100Hz control rate, 20 ticks = 200ms. If the robot hasn't reached the target

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, enabled, homing_step, joints_homed, loop_health]
+- 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, drive_faults]
 - RESPONSE: [MsgType.RESPONSE, query_type, value]
 - COMMAND:  [CmdType.XXX, ...params]
 """
@@ -92,6 +92,7 @@ class QueryType(IntEnum):
     IS_SIMULATOR = auto()
     TCP_OFFSET = auto()
     SHAPES = auto()
+    STATUS_RATE = auto()
 
 
 class CmdType(IntEnum):
@@ -159,6 +160,10 @@ class CmdType(IntEnum):
     SET_SHAPES = auto()
     # Collision-world readback query (installation + program layers)
     SHAPES = auto()
+    # Status broadcast rate: set it for a session, read it back with the
+    # control rate it divides.
+    SET_STATUS_RATE = auto()
+    STATUS_RATE = auto()
 
 
 # =============================================================================
@@ -861,6 +866,30 @@ class ActivityCmd(
     pass
 
 
+class StatusRateCmd(
+    msgspec.Struct,
+    tag=int(CmdType.STATUS_RATE),
+    array_like=True,
+    frozen=True,
+    gc=False,
+):
+    """STATUS_RATE: [CmdType.STATUS_RATE] — read the broadcast rate."""
+
+    pass
+
+
+class SetStatusRateCmd(
+    msgspec.Struct,
+    tag=int(CmdType.SET_STATUS_RATE),
+    array_like=True,
+    frozen=True,
+    gc=False,
+):
+    """SET_STATUS_RATE: [CmdType.SET_STATUS_RATE, hz]"""
+
+    hz: float
+
+
 class LoopStatsCmd(
     msgspec.Struct,
     tag=int(CmdType.LOOP_STATS),
@@ -997,6 +1026,19 @@ class StatusResultStruct(
     tool_status: list
 
 
+class StatusRateResultStruct(
+    msgspec.Struct,
+    tag=int(QueryType.STATUS_RATE),
+    array_like=True,
+    frozen=True,
+    gc=False,
+):
+    """Broadcast rate, and the control rate it divides."""
+
+    hz: float
+    control_hz: float
+
+
 class LoopStatsResultStruct(
     msgspec.Struct,
     tag=int(QueryType.LOOP_STATS),
@@ -1204,6 +1246,7 @@ class ShapesResultStruct(
 Response = (
     StatusResultStruct
     | LoopStatsResultStruct
+    | StatusRateResultStruct
     | ToolResultStruct
     | CurrentActionResultStruct
     | PingResultStruct
@@ -1364,6 +1407,7 @@ def pack_status(
     joints_homed: Sequence[int] = _NO_JOINTS_HOMED,
     p99_period_s: float = 0.0,
     overruns: int = 0,
+    drive_faults: Sequence[Sequence[str]] = (),
 ) -> bytes:
     """Pack a status broadcast message.
 
@@ -1412,6 +1456,7 @@ def pack_status(
             homing_step,
             joints_homed,
             (p99_period_s, overruns),
+            drive_faults,
         ),
         option=ormsgpack.OPT_SERIALIZE_NUMPY,
     )
@@ -1471,9 +1516,10 @@ 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.
+    # analog temperature or current registers, so this carries "faults"
+    # (one label tuple per joint) and no temperature/current series: empty
+    # lists are what tell a consumer "no such sensor" rather than "all
+    # zero", and a list of empty tuples is an all-clear.
     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
@@ -1596,7 +1642,7 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
                      tool_status_tuple, tcp_speed, simulator_active,
                      collision_active, collision_pairs, scene_epoch,
                      accepted_index, homed, enabled, homing_step, joints_homed,
-                     loop_health]
+                     loop_health, drive_faults]
 
     Args:
         data: Raw msgpack bytes
@@ -1677,6 +1723,16 @@ def decode_status_bin_into(data: bytes, buf: StatusBuffer) -> bool:
                 "p99_period_s": float(lh[0]),
                 "overruns": int(lh[1]),
             }
+        if len(msg) > 29:
+            # One label tuple per joint, empty when that drive is healthy.
+            # Absent entirely from producers that predate the field, which is
+            # what tells a consumer this backend reports no drive faults at
+            # all rather than reporting all-clear. Replaced wholesale only on
+            # change, so a snapshot's shallow dict copy keeps the labels that
+            # were current when it was taken.
+            faults = [tuple(f) for f in msg[29]]
+            if buf.drive_health.get("faults") != faults:
+                buf.drive_health["faults"] = faults
 
         return True
     except Exception as e:
@@ -1950,13 +2006,16 @@ def unpack_rx_frame_into(
     "QueueCmd",
     "ActivityCmd",
     "LoopStatsCmd",
+    "StatusRateCmd",
+    "SetStatusRateCmd",
     "ProfileCmd",
     "Command",
     # Mixin
     "MotionParamsMixin",
     # Response structs
     "StatusResultStruct",
     "LoopStatsResultStruct",
+    "StatusRateResultStruct",
     "ToolResultStruct",
     "CurrentActionResultStruct",
     "PingResultStruct",