Camera Sim Update

src/control/uav_controller.py

@@ -123,22 +123,40 @@ class Controller:
         self._drain_messages()
         print("[UAV] Mode -> GUIDED_NOGPS")
 
-    def arm(self, retries: int = 10):
+    def arm(self, retries: int = 30):
         self.set_mode_guided()
 
-        for attempt in range(1, retries + 1):
-            print(f"[UAV] Arm attempt {attempt}/{retries}...")
-            self.conn.arducopter_arm()
+        for i in range(retries):
+            print(f"[UAV] Arm attempt {i+1}/{retries}...")
 
-            t0 = time.time()
-            while time.time() - t0 < 5:
+            # Force arm: param2=21196 bypasses pre-arm checks
+            # (equivalent to MAVProxy's "arm throttle force")
+            self.conn.mav.command_long_send(
+                self.conn.target_system,
+                self.conn.target_component,
+                mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+                0,
+                1,      # param1: 1 = arm
+                21196,  # param2: force arm magic number
+                0, 0, 0, 0, 0)
+
+            # Wait up to 4 seconds for armed status instead of
+            # blocking forever with motors_armed_wait()
+            armed = False
+            for _ in range(20):
                 self._drain_messages()
-                if self.armed:
-                    print("[UAV] Armed")
-                    return True
-                sleep(0.2)
+                if self.conn.motors_armed():
+                    armed = True
+                    break
+                time.sleep(0.2)
 
-            self._drain_messages()
+            if armed:
+                self.armed = True
+                print("[UAV] Armed")
+                return True
+
+            print(f"[UAV] Arm attempt {i+1} failed, retrying...")
+            time.sleep(2)
 
         print("[UAV] FAILED to arm after all retries")
         return False
@@ -156,8 +174,12 @@ class Controller:
         print(f"[UAV] Takeoff -> {altitude}m")
 
     def land(self):
-        self.conn.set_mode_rtl()
-        print("[UAV] Landing (RTL)")
+        self.conn.mav.command_long_send(
+            self.conn.target_system,
+            self.conn.target_component,
+            MAV_CMD_DO_SET_MODE,
+            0, 89, 9, 0, 0, 0, 0, 0)
+        print("[UAV] Landing (LAND mode)")
 
     def land_at(self, lat: int, lon: int):
         self.conn.mav.command_long_send(