V2

vehicle_state.py

@@ -0,0 +1,75 @@
+"""Vehicle truth and command transactions; independent of UI/animation clocks."""
+import time
+
+
+class VehicleState:
+    HEARTBEAT_TIMEOUT = 3.0
+    COMMAND_TIMEOUT = 5.0
+
+    def __init__(self, clock=time.monotonic):
+        self.clock = clock
+        self.connected = False
+        self.armed = None
+        self.heartbeat_at = None
+        self.pending = None
+        self.error = ""
+
+    def fresh(self):
+        return (self.connected and self.heartbeat_at is not None
+                and self.clock() - self.heartbeat_at <= self.HEARTBEAT_TIMEOUT)
+
+    def heartbeat(self, armed, received_at=None):
+        received_at = self.clock() if received_at is None else received_at
+        if self.heartbeat_at is not None and received_at < self.heartbeat_at:
+            return
+        self.connected = True
+        self.heartbeat_at = received_at
+        self.armed = bool(armed)
+        # A fresh heartbeat is the authoritative confirmation of actual state.
+        # ACK ACCEPTED alone only confirms that a command was accepted.
+        if (self.pending and received_at > self.pending["started"]
+                and self.armed == (self.pending["name"] == "arm")):
+            self.pending = None
+
+    def request(self, name):
+        self.tick()
+        if name == "arm" and (not self.fresh() or self.armed is not False):
+            raise RuntimeError("Fresh, disarmed vehicle telemetry is required to engage")
+        if self.pending and (name == "arm" or self.pending["name"] == name):
+            raise RuntimeError("A vehicle confirmation is already pending")
+        now = self.clock()
+        self.pending = {"name": name, "started": now,
+                        "deadline": now + self.COMMAND_TIMEOUT, "ack": None}
+        self.error = ""
+
+    def ack(self, result, received_at=None):
+        received_at = self.clock() if received_at is None else received_at
+        if not self.pending or received_at <= self.pending["started"]:
+            return
+        self.pending["ack"] = result
+        if result not in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS"):
+            self.fail(f"{self.pending['name'].capitalize()} rejected: {result}")
+
+    def fail(self, message):
+        self.error = message
+        self.pending = None
+
+    def disconnect(self):
+        self.connected = False
+        if self.pending:
+            self.fail("Connection lost before vehicle confirmation. State unknown.")
+
+    def tick(self):
+        if self.pending and self.clock() >= self.pending["deadline"]:
+            name = self.pending["name"].capitalize()
+            self.fail(f"{name} confirmation timed out. Check current vehicle state.")
+
+    def snapshot(self):
+        self.tick()
+        fresh = self.fresh()
+        return {"armed": self.armed, "armed_known": fresh,
+                "heartbeat_age": None if self.heartbeat_at is None else self.clock() - self.heartbeat_at,
+                "pending_command": self.pending["name"] if self.pending else None,
+                "command_ack": self.pending["ack"] if self.pending else None,
+                "command_error": self.error,
+                "controls_allowed": fresh and self.armed is True and self.pending is None}