Aruco Code and UVG/UAV Logic Fixes

src/control/uav_controller.py

@@ -105,9 +105,24 @@ class Controller:
             name.encode('utf-8'),
             value,
             mavutil.mavlink.MAV_PARAM_TYPE_REAL32)
-        sleep(0.5)
+        sleep(0.1)
         self._drain_messages()
 
+    def configure_ekf_fast_converge(self):
+        """Tune EKF parameters for faster initial convergence in SITL."""
+        params = {
+            'EK3_CHECK_SCALE': 200,    # Relax EKF health checks (default 100)
+            'EK3_GPS_CHECK':   0,      # Disable GPS preflight checks in SITL
+            'EK3_POSNE_M_NSE': 0.5,    # Trust GPS position more
+            'EK3_VELD_M_NSE':  0.2,    # Trust GPS vertical vel more (default 0.4)
+            'EK3_VELNE_M_NSE': 0.1,    # Trust GPS horizontal vel more (default 0.3)
+            'INS_GYRO_CAL':    0,      # Skip gyro cal on boot (SITL doesn't need it)
+            'ARMING_CHECK':    0,      # Disable ALL preflight checks (SITL only)
+        }
+        for name, value in params.items():
+            self.set_param(name, value)
+        print("[UAV] EKF fast-converge params applied")
+
     def set_mode_guided(self):
         self.conn.mav.command_long_send(
             self.conn.target_system,
@@ -128,8 +143,33 @@ class Controller:
         self._drain_messages()
         print("[UAV] Mode -> GUIDED_NOGPS")
 
-    def arm(self, retries: int = 30):
+    def wait_for_ekf(self, timeout: float = 120.0) -> bool:
+        """Wait until EKF has actual GPS-fused position estimate."""
+        print("[UAV] Waiting for EKF position estimate ...")
+        t0 = time.time()
+        while time.time() - t0 < timeout:
+            self._drain_messages()
+            msg = self.conn.recv_match(type='EKF_STATUS_REPORT', blocking=True, timeout=2)
+            if msg is not None:
+                # bit 0x08 = pos_horiz_abs (actual GPS-fused position)
+                # bit 0x20 = pred_pos_horiz_abs (predicted, not enough for arming)
+                pos_horiz_abs = bool(msg.flags & 0x08)
+                if pos_horiz_abs:
+                    print(f"\n[UAV] EKF has position estimate (flags=0x{msg.flags:04x})")
+                    return True
+            elapsed = int(time.time() - t0)
+            print(f"\r[UAV] Waiting for EKF ... {elapsed}s  ", end='', flush=True)
+        print("\n[UAV] EKF wait timed out")
+        return False
+
+    def arm(self, retries: int = 15):
         self.set_mode_guided()
+
+        # Wait for EKF before burning through arm attempts
+        if not self.wait_for_ekf(timeout=120.0):
+            print("[UAV] Aborting arm: EKF never became healthy")
+            return False
+
         for i in range(retries):
             print(f"[UAV] Arm attempt {i+1}/{retries}...")
             self.conn.mav.command_long_send(
@@ -149,7 +189,7 @@ class Controller:
                 self.armed = True
                 print("[UAV] Armed")
                 return True
-            print(f"[UAV] Arm attempt {i+1} failed, retrying...")
+            print(f"[UAV] Arm attempt {i+1} failed, retrying in 2s...")
             time.sleep(2)
         return False
 
@@ -259,7 +299,8 @@ class Controller:
             if msg and msg.fix_type >= 3:
                 print(f"\n[UAV] GPS fix: {msg.fix_type}  sats: {msg.satellites_visible}")
                 print("[UAV] Waiting for EKF to settle ...")
-                for _ in range(15):
+                # Wait briefly; let the arming loop handle the rest
+                for _ in range(2):
                     self._drain_messages()
                     sleep(1)
                 return True