Lab040: add persistent emergency intent and safe reset

2026-08-05 13:12:49 +03:00
parent b3ebe1637f
commit 88addff5b6
18 changed files with 2513 additions and 0 deletions
--- a/protocol/persistent_emergency.py
+++ b/protocol/persistent_emergency.py
@@ -0,0 +1,147 @@
+"""Постоянное аварийное намерение наземной станции для Lab040.
+
+Автомат состояний намеренно отделяет подтверждение аварийного пакета от
+снятия намерения оператора. Только принятое подтверждение сброса разрешает
+сформировать новую команду движения.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import Enum
+
+from protocol.control_failsafe import ControlState, SAFE_STATE
+from protocol.emergency_ack import EmergencyIdentity
+
+
+class GroundState(str, Enum):
+    GROUND_NORMAL = "ground_normal"
+    GROUND_EMERGENCY_REQUESTED = "ground_emergency_requested"
+    GROUND_EMERGENCY_CONFIRMED = "ground_emergency_confirmed"
+    GROUND_RESET_REQUESTED = "ground_reset_requested"
+    GROUND_MOVEMENT_REAUTHORIZED = "ground_movement_reauthorized"
+
+
+@dataclass(frozen=True)
+class GroundSnapshot:
+    state: GroundState
+    emergency_intent: bool
+    movement_permitted: bool
+    emergency_acknowledged: bool
+    reset_acknowledged: bool
+
+
+class PersistentEmergencyController:
+    """Автомат наземной станции, не снимающий намерение по подтверждению аварии."""
+
+    def __init__(self, initial_speed_mps: float) -> None:
+        if initial_speed_mps < 0.0:
+            raise ValueError("начальная скорость не может быть отрицательной")
+        self.state = GroundState.GROUND_NORMAL
+        self.emergency_intent = False
+        self.emergency_event_id: int | None = None
+        self.emergency_stream_id: int | None = None
+        self.emergency_sequence: int | None = None
+        self.emergency_acknowledged = False
+        self.reset_request_id: int | None = None
+        self.reset_sequence: int | None = None
+        self.reset_acknowledged = False
+        self.current_motion_speed_mps = initial_speed_mps
+        self.last_operator_sequence = 0
+        self._sequence_at_emergency = -1
+
+    @property
+    def snapshot(self) -> GroundSnapshot:
+        control = self.control_state()
+        return GroundSnapshot(
+            self.state,
+            self.emergency_intent,
+            control.movement_allowed,
+            self.emergency_acknowledged,
+            self.reset_acknowledged,
+        )
+
+    def request_emergency(
+        self,
+        event_id: int,
+        stream_id: int,
+        sequence_number: int,
+    ) -> None:
+        if self.state is not GroundState.GROUND_NORMAL:
+            raise RuntimeError("аварийную команду можно запросить только из нормального состояния")
+        if event_id < 0:
+            raise ValueError("идентификатор события не может быть отрицательным")
+        self.emergency_event_id = event_id
+        self.emergency_stream_id = stream_id
+        self.emergency_sequence = sequence_number
+        self.emergency_intent = True
+        self.emergency_acknowledged = False
+        self.reset_acknowledged = False
+        self._sequence_at_emergency = self.last_operator_sequence
+        self.state = GroundState.GROUND_EMERGENCY_REQUESTED
+
+    def receive_emergency_ack(self, identity: EmergencyIdentity) -> bool:
+        expected = EmergencyIdentity(
+            self.emergency_stream_id if self.emergency_stream_id is not None else -1,
+            self.emergency_sequence if self.emergency_sequence is not None else -1,
+        )
+        if identity != expected:
+            return False
+        first = not self.emergency_acknowledged
+        self.emergency_acknowledged = True
+        if self.state is GroundState.GROUND_EMERGENCY_REQUESTED:
+            self.state = GroundState.GROUND_EMERGENCY_CONFIRMED
+        return first
+
+    def request_reset(self, reset_request_id: int, sequence_number: int) -> None:
+        if not self.emergency_intent:
+            raise RuntimeError("для сброса требуется постоянное аварийное намерение")
+        if self.state not in (
+            GroundState.GROUND_EMERGENCY_REQUESTED,
+            GroundState.GROUND_EMERGENCY_CONFIRMED,
+        ):
+            raise RuntimeError("сброс уже ожидается или завершён")
+        if reset_request_id < 0 or sequence_number < 0:
+            raise ValueError("идентификаторы сброса не могут быть отрицательными")
+        self.reset_request_id = reset_request_id
+        self.reset_sequence = sequence_number
+        self.reset_acknowledged = False
+        self.state = GroundState.GROUND_RESET_REQUESTED
+
+    def receive_reset_ack(
+        self,
+        event_id: int,
+        reset_request_id: int,
+        accepted: bool,
+    ) -> bool:
+        if self.state is not GroundState.GROUND_RESET_REQUESTED:
+            return False
+        if event_id != self.emergency_event_id or reset_request_id != self.reset_request_id:
+            return False
+        if not accepted:
+            return False
+        self.reset_acknowledged = True
+        self.emergency_intent = False
+        self.current_motion_speed_mps = 0.0
+        self.state = GroundState.GROUND_MOVEMENT_REAUTHORIZED
+        return True
+
+    def control_state(self) -> ControlState:
+        if self.state is GroundState.GROUND_NORMAL:
+            return ControlState(self.current_motion_speed_mps, 0.0, False, True)
+        return SAFE_STATE
+
+    def new_operator_motion(self, sequence_number: int, speed_mps: float) -> ControlState:
+        if self.state is not GroundState.GROUND_MOVEMENT_REAUTHORIZED:
+            raise RuntimeError("повторное движение не разрешено")
+        if sequence_number <= self._sequence_at_emergency or sequence_number <= self.last_operator_sequence:
+            raise ValueError("требуется новый номер последовательности команды оператора")
+        if speed_mps <= 0.0:
+            raise ValueError("новая скорость движения должна быть положительной")
+        self.last_operator_sequence = sequence_number
+        self.current_motion_speed_mps = speed_mps
+        self.state = GroundState.GROUND_NORMAL
+        return self.control_state()
+
+    def reject_old_operator_motion(self, sequence_number: int) -> bool:
+        return sequence_number <= self._sequence_at_emergency
--- a/protocol/safe_reset.py
+++ b/protocol/safe_reset.py
@@ -0,0 +1,192 @@
+"""Команда безопасного аварийного сброса и её подтверждение для Lab040.
+
+Оба прикладных блока данных передаются внутри неизменного пакета LinkPacket
+из Lab033.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from enum import IntEnum
+import struct
+
+from protocol.link_packet import Direction, LinkPacket, TrafficClass
+from protocol.two_stage_failsafe import SafetyState, TwoStageFailsafe
+
+
+RESET_MAGIC = b"RST1"
+RESET_ACK_MAGIC = b"RAK1"
+RESET_FORMAT = "!4sIIfB"
+RESET_ACK_FORMAT = "!4sIIBB"
+RESET_SIZE = struct.calcsize(RESET_FORMAT)
+RESET_ACK_SIZE = struct.calcsize(RESET_ACK_FORMAT)
+STREAM_RESET = 6
+STREAM_RESET_ACK = 7
+
+
+class ResetReason(IntEnum):
+    ACCEPTED = 0
+    NOT_LATCHED = 1
+    MOVING = 2
+    STALE_LINK = 3
+    EVENT_MISMATCH = 4
+    OLD_SEQUENCE = 5
+    NONZERO_REQUEST = 6
+    MOVEMENT_PERMITTED = 7
+
+
+@dataclass(frozen=True)
+class ResetCommand:
+    emergency_event_id: int
+    reset_request_id: int
+    requested_speed_mps: float
+    movement_permitted: bool
+
+
+@dataclass(frozen=True)
+class ResetAcknowledgement:
+    emergency_event_id: int
+    reset_request_id: int
+    accepted: bool
+    reason: ResetReason
+
+
+@dataclass(frozen=True)
+class ResetDecision:
+    acknowledgement: ResetAcknowledgement
+    performed: bool
+    duplicate: bool
+
+
+def encode_reset(command: ResetCommand) -> bytes:
+    if command.emergency_event_id < 0 or command.reset_request_id < 0:
+        raise ValueError("идентификаторы сброса не могут быть отрицательными")
+    return struct.pack(
+        RESET_FORMAT,
+        RESET_MAGIC,
+        command.emergency_event_id,
+        command.reset_request_id,
+        float(command.requested_speed_mps),
+        int(bool(command.movement_permitted)),
+    )
+
+
+def decode_reset(payload: bytes) -> ResetCommand:
+    if len(payload) != RESET_SIZE:
+        raise ValueError("неверная длина данных запроса сброса")
+    magic, event_id, request_id, speed, permitted = struct.unpack(RESET_FORMAT, payload)
+    if magic != RESET_MAGIC or permitted not in (0, 1):
+        raise ValueError("неверные данные запроса сброса")
+    return ResetCommand(event_id, request_id, speed, bool(permitted))
+
+
+def encode_reset_ack(ack: ResetAcknowledgement) -> bytes:
+    return struct.pack(
+        RESET_ACK_FORMAT,
+        RESET_ACK_MAGIC,
+        ack.emergency_event_id,
+        ack.reset_request_id,
+        int(ack.accepted),
+        int(ack.reason),
+    )
+
+
+def decode_reset_ack(payload: bytes) -> ResetAcknowledgement:
+    if len(payload) != RESET_ACK_SIZE:
+        raise ValueError("неверная длина подтверждения сброса")
+    magic, event_id, request_id, accepted, reason = struct.unpack(RESET_ACK_FORMAT, payload)
+    if magic != RESET_ACK_MAGIC or accepted not in (0, 1):
+        raise ValueError("неверное подтверждение сброса")
+    return ResetAcknowledgement(event_id, request_id, bool(accepted), ResetReason(reason))
+
+
+def build_reset_packet(
+    command: ResetCommand,
+    sequence_number: int,
+    generation_time_us: int,
+) -> LinkPacket:
+    return LinkPacket(
+        traffic_class=TrafficClass.EMERGENCY,
+        direction=Direction.GROUND_TO_ROVER,
+        stream_id=STREAM_RESET,
+        sequence_number=sequence_number,
+        generation_time_us=generation_time_us,
+        deadline_ms=50,
+        payload=encode_reset(command),
+    )
+
+
+def build_reset_ack(
+    decision: ResetDecision,
+    sequence_number: int,
+    generation_time_us: int,
+) -> LinkPacket:
+    return LinkPacket(
+        traffic_class=TrafficClass.EMERGENCY,
+        direction=Direction.ROVER_TO_GROUND,
+        stream_id=STREAM_RESET_ACK,
+        sequence_number=sequence_number,
+        generation_time_us=generation_time_us,
+        deadline_ms=50,
+        payload=encode_reset_ack(decision.acknowledgement),
+    )
+
+
+class SafeResetReceiver:
+    """Проверяет условия сброса и повторно подтверждает принятые дубликаты."""
+
+    def __init__(self) -> None:
+        self.last_sequence = -1
+        self.accepted_keys: set[tuple[int, int, int]] = set()
+        self.accepted = 0
+        self.rejected = 0
+        self.duplicates = 0
+        self.reacknowledgements = 0
+
+    def process(
+        self,
+        packet: LinkPacket,
+        *,
+        rover: TwoStageFailsafe,
+        actual_speed_mps: float,
+        last_link_age_seconds: float,
+        emergency_event_id: int | None,
+    ) -> ResetDecision:
+        if packet.traffic_class is not TrafficClass.EMERGENCY:
+            raise ValueError("запрос сброса должен использовать аварийный класс трафика")
+        if packet.direction is not Direction.GROUND_TO_ROVER or packet.stream_id != STREAM_RESET:
+            raise ValueError("неверная оболочка запроса сброса")
+        command = decode_reset(packet.payload)
+        key = command.emergency_event_id, command.reset_request_id, packet.sequence_number
+        if key in self.accepted_keys:
+            self.duplicates += 1
+            self.reacknowledgements += 1
+            ack = ResetAcknowledgement(command.emergency_event_id, command.reset_request_id, True, ResetReason.ACCEPTED)
+            return ResetDecision(ack, False, True)
+
+        reason = ResetReason.ACCEPTED
+        if rover.state is not SafetyState.EMERGENCY_LATCHED:
+            reason = ResetReason.NOT_LATCHED
+        elif actual_speed_mps > 1e-9:
+            reason = ResetReason.MOVING
+        elif last_link_age_seconds > 0.250 + 1e-12:
+            reason = ResetReason.STALE_LINK
+        elif command.emergency_event_id != emergency_event_id:
+            reason = ResetReason.EVENT_MISMATCH
+        elif packet.sequence_number < self.last_sequence:
+            reason = ResetReason.OLD_SEQUENCE
+        elif abs(command.requested_speed_mps) > 1e-9:
+            reason = ResetReason.NONZERO_REQUEST
+        elif command.movement_permitted:
+            reason = ResetReason.MOVEMENT_PERMITTED
+
+        self.last_sequence = max(self.last_sequence, packet.sequence_number)
+        accepted = reason is ResetReason.ACCEPTED
+        if accepted:
+            self.accepted_keys.add(key)
+            self.accepted += 1
+            rover.state = SafetyState.NORMAL
+        else:
+            self.rejected += 1
+        ack = ResetAcknowledgement(command.emergency_event_id, command.reset_request_id, accepted, reason)
+        return ResetDecision(ack, accepted, False)