V2

SUB Control/web_ui.py

@@ -16,6 +16,7 @@ import webview
 from serial.tools import list_ports
 
 from app import Axes, MOTOR_FUNCTIONS, MavlinkWorker, motor_number_for_function
+from vehicle_state import VehicleState
 
 
 class ControlApi:
@@ -24,8 +25,12 @@ class ControlApi:
     def __init__(self):
         self.events: queue.Queue = queue.Queue()
         self.worker = MavlinkWorker(self.events)
+        self.worker.require_input_lease = True
+        self.vehicle_state = VehicleState()
+        self.battery_received_at = None
         self.demo_mode = False
         self.lock = threading.Lock()
+        self._poll_lock = threading.Lock()
         self.state = {
             "connected": False,
             "link": "DISCONNECTED",
@@ -240,13 +245,13 @@ class ControlApi:
                     }
             return self._snapshot()
         self.worker.rate_hz = int(rate)
-        self.worker.connect(str(port), int(baud))
         with self.lock:
+            self.vehicle_state.disconnect()
             self.state.update(
                 connected=True, link="CONNECTING",
                 status=f"Opening {port} at {baud} baud…",
-                armed=False,
             )
+        self.worker.connect(str(port), int(baud))
         print(f"[web_ui] connect() dispatched to worker thread OK")
         # Return only once the worker has either seen a heartbeat or reported a
         # connection failure. This makes the button independent of the regular
@@ -272,7 +277,7 @@ class ControlApi:
         self.worker.disconnect()
         return True
 
-    def set_axes(self, forward, turn, vertical):
+    def set_axes(self, forward, turn, vertical, sequence=None):
         if self.demo_mode:
             with self.lock:
                 self.state["status"] = (
@@ -280,12 +285,20 @@ class ControlApi:
                     f"yaw {float(turn):+.2f} • heave {float(vertical):+.2f}"
                 )
             return True
-        self.worker.set_axes(Axes(
-            max(-1.0, min(1.0, float(forward))),
-            0.0,
-            max(-1.0, min(1.0, float(vertical))),
-            max(-1.0, min(1.0, float(turn))),
-        ))
+        with self.lock:
+            allowed = self.vehicle_state.snapshot()["controls_allowed"]
+            self.worker.set_axes(Axes(
+                max(-1.0, min(1.0, float(forward))), 0.0,
+                max(-1.0, min(1.0, float(vertical))),
+                max(-1.0, min(1.0, float(turn))),
+            ) if allowed else Axes(), sequence)
+        return True
+
+    def stop_controls(self, sequence=None):
+        """Invalidate queued input without claiming that the vehicle disarmed."""
+        with self.lock:
+            self.worker.set_axes(Axes(), sequence)
+            self.worker._send_control(force_neutral=True)
         return True
 
     def set_gain(self, value):
@@ -293,7 +306,7 @@ class ControlApi:
         self.worker.gain = value / 100.0
         return value
 
-    def command(self, name):
+    def command(self, name, sequence=None, expires_at=None):
         if self.demo_mode:
             if name not in ("manual", "arm", "disarm"):
                 raise ValueError(f"Unknown command: {name}")
@@ -311,15 +324,25 @@ class ControlApi:
             elif name == "disarm":
                 raise RuntimeError("Connect to vehicle before disarming")
             raise RuntimeError("No autopilot heartbeat")
+        if name in ("arm", "disarm"):
+            self.poll()
+            with self.lock:
+                if name == "arm":
+                    if expires_at is not None and time.time() * 1000 > expires_at:
+                        raise RuntimeError("Engage request expired before it could be sent")
+                    if sequence is not None and sequence <= self.worker.last_input_sequence:
+                        raise RuntimeError("Engage canceled by a newer input reset")
+                self.vehicle_state.request(name)
+                self.worker.set_axes(Axes(), sequence)
+                try:
+                    self.worker.command_arm(name == "arm")
+                except Exception as exc:
+                    self.vehicle_state.fail(f"{name.capitalize()} send failed: {exc}")
+                    raise
+            return self._snapshot()
         if name == "manual":
             self.worker.log("OPERATOR", "clicked SET MANUAL")
             self.worker.set_manual_mode()
-        elif name == "arm":
-            self.worker.log("OPERATOR", "clicked ARM")
-            self.worker.command_arm(True)
-        elif name == "disarm":
-            self.worker.log("OPERATOR", "clicked DISARM")
-            self.worker.command_arm(False)
         else:
             raise ValueError(f"Unknown command: {name}")
         return True
@@ -377,13 +400,15 @@ class ControlApi:
         return True
 
     def poll(self):
-        while True:
-            try:
-                event = self.events.get_nowait()
-            except queue.Empty:
-                break
-            self._consume(event)
-        if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 5:
+        # Bound a poll even if telemetry arrives continuously.
+        with self._poll_lock:
+            for _ in range(500):
+                try:
+                    event = self.events.get_nowait()
+                except queue.Empty:
+                    break
+                self._consume(event)
+        if self.worker.target_system and time.monotonic() - self.worker.last_heartbeat > 3:
             with self.lock:
                 self.state["link"] = (
                     "DATA OK / HEARTBEAT DELAYED"
@@ -407,14 +432,20 @@ class ControlApi:
         with self.lock:
             snap = json.loads(json.dumps(self.state))
             snap["app_state"] = self._derive_app_state()
-            snap["armed"] = bool(self.state.get("armed", False))
+            if not self.demo_mode:
+                snap.update(self.vehicle_state.snapshot())
+                if snap["connected"] and snap["app_state"] != "connecting" and not snap["armed_known"]:
+                    snap["app_state"] = "link_degraded"
+                snap["battery_stale"] = (self.battery_received_at is None
+                    or time.monotonic() - self.battery_received_at > 5)
             return snap
 
     def _consume(self, event):
         kind = event[0]
         with self.lock:
             if kind == "heartbeat":
-                _, system, autopilot, mode, armed = event
+                _, system, autopilot, mode, armed = event[:5]
+                self.vehicle_state.heartbeat(armed, event[5] if len(event) > 5 else None)
                 self.state.update(
                     connected=True, link="LINK OK", vehicle=f"{mode} • {'ARMED' if armed else 'DISARMED'}",
                     armed=bool(armed),
@@ -423,8 +454,11 @@ class ControlApi:
             elif kind in ("status", "statustext"):
                 self.state["status"] = event[1]
             elif kind == "ack":
+                if event[1] == 400:
+                    self.vehicle_state.ack(event[2], event[3] if len(event) > 3 else None)
                 self.state["status"] = f"Command {event[1]} ACK: {event[2]}"
             elif kind == "battery":
+                self.battery_received_at = time.monotonic()
                 voltage = float(event[1])
                 cells = self.battery_config["cells"]
                 per_cell = voltage / cells
@@ -452,10 +486,11 @@ class ControlApi:
                 self.state["status"] = event[1]
                 self.state["last_error"] = event[1]
             elif kind == "disconnected":
+                self.vehicle_state.disconnect()
                 last_err = self.state.get("last_error")
                 status_msg = last_err if last_err else "Disconnected"
                 self.state.update(connected=False, link="DISCONNECTED", vehicle="No heartbeat",
-                                  armed=False, mode=None, battery=None, status=status_msg)
+                                  mode=None, status=status_msg)
                 self.state["last_error"] = None
 
     def _update_servo(self, servo):
@@ -495,7 +530,7 @@ def _apply_dwm_styling(window):
 
 
 def _get_window_size() -> tuple[int, int, tuple[int, int]]:
-    target_w, target_h = 1280, 750
+    target_w, target_h = 1100, 650
     min_w, min_h = 960, 580
     try:
         import ctypes