Add reliable built-in demo mode

SUB Control/app.py

@@ -42,6 +42,25 @@ class Axes:
     yaw: float = 0.0
 
 
+def normalize_endpoint(port: str) -> str:
+    p = str(port).strip()
+    if not p:
+        return p
+    lower = p.lower()
+    if "sim" in lower or "5760" in lower or lower in ("tcp", "tcp:", "tcp:127.0.0.1", "127.0.0.1"):
+        return "tcp:127.0.0.1:5760"
+    if "14550" in lower or lower in ("udp", "udp:", "udpin", "udpin:"):
+        return "udpin:127.0.0.1:14550"
+    if p.startswith("127.0.0.1:") or p.startswith("localhost:"):
+        num = p.split(":")[1]
+        if num == "14550":
+            return f"udpin:{p}"
+        return f"tcp:{p}"
+    if ":" in p and not p.startswith(("tcp:", "tcpin:", "udp:", "udpin:", "udpout:")):
+        return f"tcp:{p}"
+    return p
+
+
 class MavlinkWorker:
     def __init__(self, events: queue.Queue):
         self.events = events
@@ -74,29 +93,52 @@ class MavlinkWorker:
         self.log_path = None
 
     def connect(self, port: str, baud: int):
+        print(f"[MavlinkWorker] connect() called: port={port!r}, baud={baud}")
         if self.thread and self.thread.is_alive():
+            print(f"[MavlinkWorker] Thread already alive, ignoring connect()")
             return
         self.stop_event.clear()
         log_dir = Path(__file__).resolve().parent / "logs"
         log_dir.mkdir(exist_ok=True)
         self.log_path = log_dir / f"ardusub_{datetime.now():%Y%m%d_%H%M%S}.log"
         self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1)
-        self.log("SESSION", f"start port={port} baud={baud} control_hz={self.rate_hz}")
+        normalized = normalize_endpoint(port)
+        print(f"[MavlinkWorker] normalized endpoint: {normalized!r}")
+        self.log("SESSION", f"start port={normalized} (input={port}) baud={baud} control_hz={self.rate_hz}")
         self.events.put(("log_path", str(self.log_path)))
         self.thread = threading.Thread(
-            target=self._run, args=(port, baud), daemon=True
+            target=self._run, args=(normalized, baud), daemon=True
         )
         self.thread.start()
+        print(f"[MavlinkWorker] worker thread started")
 
     def _run(self, port: str, baud: int):
+        print(f"[MavlinkWorker._run] ENTERED: port={port!r}, baud={baud}")
         try:
+            print(f"[MavlinkWorker._run] Creating mavlink_connection...")
             conn = mavutil.mavlink_connection(
                 port, baud=baud, source_system=255, source_component=190,
-                autoreconnect=False, force_connected=True,
+                # Fail a missing TCP simulator immediately. pymavlink otherwise
+                # retries the refused socket several times and leaves the UI in
+                # "Opening" long after the actual failure.
+                autoreconnect=False, retries=0, force_connected=True,
             )
+            print(f"[MavlinkWorker._run] mavlink_connection created: {type(conn).__name__}")
             self.connection = conn
-            self.events.put(("status", f"Port open; waiting for heartbeat at {baud} baud…"))
-            next_gcs_hb = 0.0
+            if port.startswith("tcp:"):
+                self.events.put(("status", f"Connected to {port}; waiting for vehicle heartbeat…"))
+            else:
+                self.events.put(("status", f"Port {port} open; waiting for heartbeat at {baud} baud…"))
+
+            # Send initial GCS heartbeat immediately
+            self.log("TX_HEARTBEAT", "GCS initial")
+            conn.mav.heartbeat_send(
+                mavutil.mavlink.MAV_TYPE_GCS,
+                mavutil.mavlink.MAV_AUTOPILOT_INVALID,
+                0, 0, mavutil.mavlink.MAV_STATE_ACTIVE,
+            )
+            print(f"[MavlinkWorker._run] Initial heartbeat sent, entering recv loop")
+            next_gcs_hb = time.monotonic() + 1.0
             next_control = 0.0
             while not self.stop_event.is_set():
                 now = time.monotonic()
@@ -120,6 +162,7 @@ class MavlinkWorker:
                 else:
                     time.sleep(0.01)
         except Exception as exc:
+            print(f"[MavlinkWorker._run] EXCEPTION: {exc}")
             self.events.put(("error", f"Connection error: {exc}"))
         finally:
             self._safe_stop()
@@ -156,7 +199,7 @@ class MavlinkWorker:
         except Exception:
             payload = str(msg)
         self.log("RX", payload)
-        if msg_type == "HEARTBEAT" and msg.get_srcComponent() == 1:
+        if msg_type == "HEARTBEAT" and getattr(msg, "type", 0) != mavutil.mavlink.MAV_TYPE_GCS:
             first = not self.target_system
             self.target_system = msg.get_srcSystem()
             self.target_component = msg.get_srcComponent()
@@ -887,6 +930,9 @@ class App(tk.Tk):
 
     def refresh_ports(self):
         ports = [p.device for p in list_ports.comports()]
+        for sim in ("tcp:127.0.0.1:5760", "udpin:127.0.0.1:14550"):
+            if sim not in ports:
+                ports.append(sim)
         self.port.configure(values=ports)
         if ports and self.port.get() not in ports:
             self.port.set(ports[0])