feat: Add support for GPS-denied operation with configurable parameters and force arming.

src/drone_controller.py

@@ -1,6 +1,6 @@
 #!/usr/bin/env python3
 """
-Simple Drone Controller - Takeoff, Fly Pattern, Land
+Drone Controller - Takeoff, Fly Pattern, Land
 =====================================================
 A minimal controller that demonstrates drone movement in simulation.
 
@@ -28,8 +28,8 @@ except ImportError:
 from config import MAVLINK
 
 
-class SimpleDroneController:
-    """Simple drone controller with takeoff, fly pattern, and land."""
+class DroneController:
+    """Drone controller with takeoff, fly pattern, and land."""
     
     def __init__(self, connection_string=None):
         self.connection_string = connection_string or MAVLINK["connection_string"]
@@ -77,27 +77,58 @@ class SimpleDroneController:
         mode_map = self.mav.mode_mapping()
         if mode_name.upper() not in mode_map:
             print(f"[ERROR] Unknown mode: {mode_name}")
+            print(f"[INFO] Available modes: {list(mode_map.keys())}")
             return False
         
         mode_id = mode_map[mode_name.upper()]
         self.mav.set_mode(mode_id)
-        time.sleep(0.5)
+        time.sleep(1)
         self.update_state()
         print(f"[OK] Mode: {self.mode}")
         return True
     
+    def set_param(self, param_name, value):
+        """Set a parameter value."""
+        self.mav.mav.param_set_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            param_name.encode('utf-8'),
+            float(value),
+            mavutil.mavlink.MAV_PARAM_TYPE_REAL32
+        )
+        time.sleep(0.5)
+    
+    def setup_gps_denied(self):
+        """Configure for GPS-denied operation."""
+        print("[INFO] Configuring for GPS-denied operation...")
+        
+        # Disable GPS arming check
+        self.set_param("ARMING_CHECK", 0)  # Disable all arming checks
+        
+        # Set EKF to not require GPS
+        self.set_param("EK3_SRC1_POSXY", 0)  # Disable GPS for position
+        self.set_param("EK3_SRC1_VELXY", 0)  # Disable GPS for velocity
+        self.set_param("EK3_SRC1_POSZ", 1)   # Use barometer for altitude
+        
+        print("[OK] GPS-denied parameters set")
+        time.sleep(1)
+    
     def arm(self):
-        """Force arm the drone."""
+        """Force arm the drone (bypasses pre-arm checks)."""
         print("[INFO] Arming...")
         
         for attempt in range(3):
+            # Force arm command
             self.mav.mav.command_long_send(
                 self.mav.target_system,
                 self.mav.target_component,
                 mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-                0, 1, 21196, 0, 0, 0, 0, 0  # 21196 = force arm
+                0,
+                1,      # Arm
+                21196,  # Force arm magic number
+                0, 0, 0, 0, 0
             )
-            time.sleep(1)
+            time.sleep(2)
             self.update_state()
             
             if self.armed:
@@ -106,6 +137,7 @@ class SimpleDroneController:
             print(f"[WARN] Arm attempt {attempt + 1}/3...")
         
         print("[ERROR] Failed to arm")
+        print("[TIP] Try in MAVProxy: arm throttle force")
         return False
     
     def takeoff(self, altitude=5.0):
@@ -208,7 +240,7 @@ class SimpleDroneController:
 
 
 def main():
-    parser = argparse.ArgumentParser(description="Simple Drone Controller")
+    parser = argparse.ArgumentParser(description="Simple Drone Controller - GPS Denied")
     parser.add_argument("--pattern", "-p", choices=["square", "circle", "hover"], 
                         default="square", help="Flight pattern")
     parser.add_argument("--altitude", "-a", type=float, default=5.0,
@@ -217,11 +249,16 @@ def main():
                         help="Pattern size/radius (meters)")
     parser.add_argument("--connection", "-c", default=None,
                         help="MAVLink connection string")
+    parser.add_argument("--gps", action="store_true",
+                        help="Use GPS mode (default is GPS-denied)")
     args = parser.parse_args()
     
+    gps_mode = "with GPS" if args.gps else "GPS-DENIED"
+    
     print("\n" + "=" * 50)
-    print("  Simple Drone Controller")
+    print("  Drone Controller")
     print("=" * 50)
+    print(f"  Mode: {gps_mode}")
     print(f"  Pattern: {args.pattern}")
     print(f"  Altitude: {args.altitude}m")
     print(f"  Size: {args.size}m")
@@ -237,11 +274,19 @@ def main():
     try:
         # Setup
         print("\n--- SETUP ---")
-        drone.set_mode("GUIDED")
+        
+        # Configure for GPS-denied if needed
+        if not args.gps:
+            drone.setup_gps_denied()
+            drone.set_mode("GUIDED_NOGPS")
+        else:
+            drone.set_mode("GUIDED")
+        
         time.sleep(1)
         
         if not drone.arm():
             print("[ERROR] Could not arm")
+            print("[TIP] In MAVProxy console, try: arm throttle force")
             sys.exit(1)
         
         # Takeoff