Lab040: add persistent emergency intent and safe reset

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)