feat: Add vehicle type detection and robust flight mode setting based on vehicle type.

src/drone_controller.py

@@ -38,6 +38,7 @@ class DroneController:
         self.mode = "UNKNOWN"
         self.altitude = 0
         self.position = {"x": 0, "y": 0, "z": 0}
+        self.vehicle_type = "copter"  # Default to copter
         
     def connect(self, timeout=30):
         """Connect to ArduPilot SITL."""
@@ -45,8 +46,25 @@ class DroneController:
         
         try:
             self.mav = mavutil.mavlink_connection(self.connection_string)
-            self.mav.wait_heartbeat(timeout=timeout)
+            msg = self.mav.wait_heartbeat(timeout=timeout)
             print(f"[OK] Connected to system {self.mav.target_system}")
+            
+            # Detect vehicle type from heartbeat
+            if msg:
+                mav_type = msg.type
+                if mav_type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
+                    self.vehicle_type = "copter"
+                    print(f"[INFO] Vehicle: Quadrotor (ArduCopter)")
+                elif mav_type == mavutil.mavlink.MAV_TYPE_HELICOPTER:
+                    self.vehicle_type = "copter"
+                    print(f"[INFO] Vehicle: Helicopter")
+                elif mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
+                    self.vehicle_type = "plane"
+                    print(f"[INFO] Vehicle: Fixed Wing (ArduPlane)")
+                else:
+                    self.vehicle_type = "copter"  # Default to copter
+                    print(f"[INFO] Vehicle type: {mav_type}")
+            
             return True
         except Exception as e:
             print(f"[ERROR] Connection failed: {e}")
@@ -74,16 +92,54 @@ class DroneController:
     
     def set_mode(self, mode_name):
         """Set flight mode."""
-        mode_map = self.mav.mode_mapping()
         mode_upper = mode_name.upper()
         
-        if mode_upper not in mode_map:
-            print(f"[ERROR] Unknown mode: {mode_name}")
-            print(f"[INFO] Available modes: {list(mode_map.keys())}")
-            return False
+        # ArduCopter mode IDs (hardcoded as fallback since pymavlink can be buggy)
+        copter_modes = {
+            'STABILIZE': 0,
+            'ACRO': 1,
+            'ALT_HOLD': 2,
+            'AUTO': 3,
+            'GUIDED': 4,
+            'LOITER': 5,
+            'RTL': 6,
+            'CIRCLE': 7,
+            'LAND': 9,
+            'DRIFT': 11,
+            'SPORT': 13,
+            'FLIP': 14,
+            'AUTOTUNE': 15,
+            'POSHOLD': 16,
+            'BRAKE': 17,
+            'THROW': 18,
+            'AVOID_ADSB': 19,
+            'GUIDED_NOGPS': 20,
+            'SMART_RTL': 21,
+        }
         
+        # Try copter modes first if we detected a copter
+        if hasattr(self, 'vehicle_type') and self.vehicle_type == 'copter':
+            if mode_upper in copter_modes:
+                mode_id = copter_modes[mode_upper]
+            else:
+                print(f"[ERROR] Unknown copter mode: {mode_name}")
+                print(f"[INFO] Available modes: {list(copter_modes.keys())}")
+                return False
+        else:
+            # Fall back to pymavlink mode mapping
+            mode_map = self.mav.mode_mapping()
+            if mode_upper not in mode_map:
+                # Try copter modes anyway
+                if mode_upper in copter_modes:
+                    mode_id = copter_modes[mode_upper]
+                else:
+                    print(f"[ERROR] Unknown mode: {mode_name}")
+                    return False
+            else:
                 mode_id = mode_map[mode_upper]
         
+        print(f"[INFO] Setting mode to {mode_upper} (id={mode_id})...")
+        
         # Send mode change using MAV_CMD_DO_SET_MODE
         self.mav.mav.command_long_send(
             self.mav.target_system,
@@ -104,7 +160,7 @@ class DroneController:
                 return True
             time.sleep(0.2)
         
-        print(f"[OK] Mode: {self.mode}")
+        print(f"[OK] Mode set (current: {self.mode})")
         return True
     
     def set_param(self, param_name, value):