Lab038: add control failsafe and emergency acknowledgement
This commit is contained in:
LittleSam129
132
protocol/control_failsafe.py
Normal file
132
protocol/control_failsafe.py
Normal file
@@ -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
|
||||
92
protocol/emergency_ack.py
Normal file
92
protocol/emergency_ack.py
Normal file
@@ -0,0 +1,92 @@
|
||||
"""Emergency-command acknowledgement carried by unchanged Lab033 packets."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
import struct
|
||||
|
||||
from protocol.link_packet import Direction, LinkPacket, TrafficClass
|
||||
|
||||
|
||||
ACK_MAGIC = b"ACK1"
|
||||
ACK_PAYLOAD_FORMAT = "!4sHI"
|
||||
ACK_PAYLOAD_SIZE = struct.calcsize(ACK_PAYLOAD_FORMAT)
|
||||
STREAM_EMERGENCY_ACK = 5
|
||||
|
||||
|
||||
class EmergencyAckError(ValueError):
|
||||
"""An emergency acknowledgement is malformed."""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class EmergencyIdentity:
|
||||
stream_id: int
|
||||
sequence_number: int
|
||||
|
||||
|
||||
def encode_ack_payload(identity: EmergencyIdentity) -> bytes:
|
||||
if not 0 <= identity.stream_id <= 0xFFFF:
|
||||
raise EmergencyAckError("emergency stream_id is outside uint16")
|
||||
if not 0 <= identity.sequence_number <= 0xFFFFFFFF:
|
||||
raise EmergencyAckError("emergency sequence_number is outside uint32")
|
||||
return struct.pack(ACK_PAYLOAD_FORMAT, ACK_MAGIC, identity.stream_id, identity.sequence_number)
|
||||
|
||||
|
||||
def decode_ack_payload(payload: bytes) -> EmergencyIdentity:
|
||||
if not isinstance(payload, (bytes, bytearray)):
|
||||
raise TypeError("payload must be bytes or bytearray")
|
||||
if len(payload) != ACK_PAYLOAD_SIZE:
|
||||
raise EmergencyAckError("invalid acknowledgement payload length")
|
||||
magic, stream_id, sequence_number = struct.unpack(ACK_PAYLOAD_FORMAT, bytes(payload))
|
||||
if magic != ACK_MAGIC:
|
||||
raise EmergencyAckError("invalid acknowledgement payload magic")
|
||||
return EmergencyIdentity(stream_id, sequence_number)
|
||||
|
||||
|
||||
def build_emergency_ack(command: LinkPacket, ack_sequence: int, generation_time_us: int) -> LinkPacket:
|
||||
"""Build a high-priority rover-to-ground ACK for one emergency command."""
|
||||
|
||||
if command.traffic_class is not TrafficClass.EMERGENCY:
|
||||
raise ValueError("acknowledged packet must be an emergency command")
|
||||
return LinkPacket(
|
||||
traffic_class=TrafficClass.EMERGENCY,
|
||||
direction=Direction.ROVER_TO_GROUND,
|
||||
stream_id=STREAM_EMERGENCY_ACK,
|
||||
sequence_number=ack_sequence,
|
||||
generation_time_us=generation_time_us,
|
||||
deadline_ms=50,
|
||||
payload=encode_ack_payload(EmergencyIdentity(command.stream_id, command.sequence_number)),
|
||||
)
|
||||
|
||||
|
||||
def acknowledged_identity(packet: LinkPacket) -> EmergencyIdentity:
|
||||
"""Validate ACK envelope fields and return the referenced command."""
|
||||
|
||||
if packet.traffic_class is not TrafficClass.EMERGENCY:
|
||||
raise EmergencyAckError("ack must use high-priority traffic class")
|
||||
if packet.direction is not Direction.ROVER_TO_GROUND:
|
||||
raise EmergencyAckError("ack must travel rover to ground")
|
||||
if packet.stream_id != STREAM_EMERGENCY_ACK:
|
||||
raise EmergencyAckError("unexpected acknowledgement stream")
|
||||
return decode_ack_payload(packet.payload)
|
||||
|
||||
|
||||
class EmergencyAckReceiver:
|
||||
"""Recognize first and late acknowledgements independently per command."""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self._received: set[EmergencyIdentity] = set()
|
||||
self.unique = 0
|
||||
self.duplicates = 0
|
||||
|
||||
def accept(self, packet: LinkPacket) -> bool:
|
||||
identity = acknowledged_identity(packet)
|
||||
if identity in self._received:
|
||||
self.duplicates += 1
|
||||
return False
|
||||
self._received.add(identity)
|
||||
self.unique += 1
|
||||
return True
|
||||
|
||||
def has_received(self, identity: EmergencyIdentity) -> bool:
|
||||
return identity in self._received
|
||||
Reference in New Issue
Block a user