Lab038: add control failsafe and emergency acknowledgement

protocol/control_failsafe.py

@@ -0,0 +1,132 @@
+"""Local rover command state and watchdog safety logic for Lab038.
+
+The educational control payload is carried inside the unchanged Lab033 common
+packet.  It is intentionally small and is not a production motor-controller
+protocol.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+import struct
+
+from protocol.link_packet import LinkPacket, TrafficClass
+
+
+CONTROL_MAGIC = b"CTL1"
+CONTROL_PAYLOAD_FORMAT = "!4sffBB"
+CONTROL_PAYLOAD_SIZE = struct.calcsize(CONTROL_PAYLOAD_FORMAT)
+
+
+class ControlPayloadError(ValueError):
+    """The educational Lab038 control payload is malformed."""
+
+
+@dataclass(frozen=True)
+class ControlState:
+    desired_speed_mps: float
+    desired_turn: float
+    braking: bool
+    movement_allowed: bool
+
+
+SAFE_STATE = ControlState(0.0, 0.0, True, False)
+
+
+def encode_control_state(state: ControlState) -> bytes:
+    """Serialize the Lab038 teaching payload without changing LinkPacket."""
+
+    if not isinstance(state, ControlState):
+        raise TypeError("state must be ControlState")
+    return struct.pack(
+        CONTROL_PAYLOAD_FORMAT,
+        CONTROL_MAGIC,
+        float(state.desired_speed_mps),
+        float(state.desired_turn),
+        int(bool(state.braking)),
+        int(bool(state.movement_allowed)),
+    )
+
+
+def decode_control_state(payload: bytes) -> ControlState:
+    """Decode and validate the Lab038 teaching payload."""
+
+    if not isinstance(payload, (bytes, bytearray)):
+        raise TypeError("payload must be bytes or bytearray")
+    if len(payload) != CONTROL_PAYLOAD_SIZE:
+        raise ControlPayloadError("invalid control payload length")
+    magic, speed, turn, braking, allowed = struct.unpack(CONTROL_PAYLOAD_FORMAT, bytes(payload))
+    if magic != CONTROL_MAGIC:
+        raise ControlPayloadError("invalid control payload magic")
+    if braking not in (0, 1) or allowed not in (0, 1):
+        raise ControlPayloadError("control booleans must be zero or one")
+    return ControlState(speed, turn, bool(braking), bool(allowed))
+
+
+class RoverControlFailsafe:
+    """Strictly-monotonic command receiver with temporary and latched stops."""
+
+    def __init__(self, watchdog_timeout_us: int, boot_time_us: int = 0) -> None:
+        if watchdog_timeout_us <= 0:
+            raise ValueError("watchdog_timeout_us must be positive")
+        self.watchdog_timeout_us = watchdog_timeout_us
+        self.last_sequence = -1
+        self.last_new_command_time_us = boot_time_us
+        self.requested_state = SAFE_STATE
+        self.temporary_safe_stop = False
+        self.emergency_stop_latched = False
+        self.stale_commands = 0
+        self.new_commands = 0
+        self.watchdog_triggers = 0
+        self.emergency_actions = 0
+        self.emergency_duplicates = 0
+        self._emergency_keys: set[tuple[int, int]] = set()
+
+    @property
+    def effective_state(self) -> ControlState:
+        if self.emergency_stop_latched or self.temporary_safe_stop:
+            return SAFE_STATE
+        return self.requested_state
+
+    def receive_state(self, packet: LinkPacket, receive_time_us: int) -> bool:
+        """Apply a strictly newer normal command and refresh the watchdog."""
+
+        if packet.traffic_class is not TrafficClass.CONTROL:
+            raise ValueError("normal state must use CONTROL traffic class")
+        state = decode_control_state(packet.payload)
+        if packet.sequence_number <= self.last_sequence:
+            self.stale_commands += 1
+            return False
+        self.last_sequence = packet.sequence_number
+        self.last_new_command_time_us = receive_time_us
+        self.requested_state = state
+        self.temporary_safe_stop = False
+        self.new_commands += 1
+        return True
+
+    def check_watchdog(self, now_us: int) -> bool:
+        """Enter a temporary safe stop when fresh control has timed out."""
+
+        if now_us < self.last_new_command_time_us:
+            raise ValueError("watchdog time moved backwards")
+        expired = now_us - self.last_new_command_time_us >= self.watchdog_timeout_us
+        if expired and not self.temporary_safe_stop:
+            self.temporary_safe_stop = True
+            self.watchdog_triggers += 1
+            return True
+        return False
+
+    def receive_emergency(self, packet: LinkPacket) -> bool:
+        """Latch emergency stop once; return false for subsequent copies."""
+
+        if packet.traffic_class is not TrafficClass.EMERGENCY:
+            raise ValueError("emergency command must use EMERGENCY traffic class")
+        key = packet.stream_id, packet.sequence_number
+        if key in self._emergency_keys:
+            self.emergency_duplicates += 1
+            return False
+        self._emergency_keys.add(key)
+        self.emergency_stop_latched = True
+        self.temporary_safe_stop = True
+        self.emergency_actions += 1
+        return True