76 lines
3.1 KiB
Python
76 lines
3.1 KiB
Python
"""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}
|