133 lines
4.7 KiB
Python
133 lines
4.7 KiB
Python
"""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
|