Controller Update 3

scripts/run_autonomous.sh

@@ -22,17 +22,22 @@ print_error() { echo -e "${RED}[ERROR]${NC} $1"; }
 SOFTWARE_RENDER=false
 WORLD="iris_runway.sdf"
 MISSION="hover"  # hover, square, circle
+USE_CONSOLE=true  # Show MAVProxy console by default
 
 while [[ $# -gt 0 ]]; do
     case $1 in
         --software-render) SOFTWARE_RENDER=true; shift ;;
         --world) WORLD="$2"; shift 2 ;;
         --mission) MISSION="$2"; shift 2 ;;
+        --no-console) USE_CONSOLE=false; shift ;;
+        --console) USE_CONSOLE=true; shift ;;
         -h|--help)
             echo "Usage: $0 [options]"
             echo "  --software-render   Use software rendering (WSL/no GPU)"
             echo "  --world <file>      World file to load (default: iris_runway.sdf)"
             echo "  --mission <type>    Mission type: hover, square, circle (default: hover)"
+            echo "  --console           Show MAVProxy console (default: on)"
+            echo "  --no-console        Hide MAVProxy console"
             exit 0
             ;;
         *) shift ;;
@@ -45,6 +50,7 @@ cleanup() {
     pkill -f "gz sim" 2>/dev/null || true
     pkill -f "arducopter" 2>/dev/null || true
     pkill -f "sim_vehicle.py" 2>/dev/null || true
+    pkill -f "mavproxy" 2>/dev/null || true
     pkill -f "autonomous_controller.py" 2>/dev/null || true
 }
 trap cleanup EXIT
@@ -87,15 +93,26 @@ print_success "Gazebo running (PID: $GZ_PID)"
 print_info "Starting ArduPilot SITL..."
 cd ~/ardupilot
 
+# Build SITL command
+SITL_ARGS="-v ArduCopter -f gazebo-iris --model JSON -I0"
+
+# Add console/map if requested
+if [ "$USE_CONSOLE" = true ]; then
+    SITL_ARGS="$SITL_ARGS --console --map"
+    print_info "MAVProxy console enabled"
+else
+    SITL_ARGS="$SITL_ARGS --no-mavproxy"
+fi
+
 # Check if custom param file exists
 PARAM_FILE="$PROJECT_DIR/config/ardupilot_gps_denied.parm"
 if [ -f "$PARAM_FILE" ]; then
     print_info "Loading GPS-denied parameters..."
-    sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 \
+    SITL_ARGS="$SITL_ARGS --add-param-file $PARAM_FILE"
-        --add-param-file "$PARAM_FILE" &
-else
-    sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 &
 fi
+
+# Start SITL
+sim_vehicle.py $SITL_ARGS &
 SITL_PID=$!
 
 # Wait longer for SITL to fully initialize

src/autonomous_controller.py

@@ -3,6 +3,9 @@
 Autonomous UAV Controller using pymavlink.
 Connects directly to ArduPilot SITL and controls the drone autonomously.
 No ROS/MAVROS required.
+
+Uses GUIDED_NOGPS mode for GPS-denied flight simulation while keeping
+GPS enabled in EKF for geofence safety.
 """
 
 import time
@@ -14,6 +17,20 @@ from pymavlink import mavutil
 class AutonomousController:
     """Autonomous UAV controller using pymavlink."""
     
+    # ArduCopter mode IDs
+    COPTER_MODES = {
+        'STABILIZE': 0,
+        'ACRO': 1,
+        'ALT_HOLD': 2,
+        'AUTO': 3,
+        'GUIDED': 4,
+        'LOITER': 5,
+        'RTL': 6,
+        'CIRCLE': 7,
+        'LAND': 9,
+        'GUIDED_NOGPS': 20,
+    }
+    
     def __init__(self, connection_string='tcp:127.0.0.1:5760'):
         """Initialize connection to ArduPilot."""
         print(f"[CTRL] Connecting to {connection_string}...")
@@ -25,255 +42,363 @@ class AutonomousController:
         print(f"[CTRL] Connected! System {self.mav.target_system} Component {self.mav.target_component}")
         
         self.home_position = None
+        self.altitude = 0
+        self.position = {"x": 0, "y": 0, "z": 0}
+        self.armed = False
         
-    def send_vision_position(self, x=0.0, y=0.0, z=0.0):
+        # Request data streams
-        """Send vision position estimate to help EKF initialize."""
+        self.mav.mav.request_data_stream_send(
-        usec = int(time.time() * 1e6)
+            self.mav.target_system,
-        self.mav.mav.vision_position_estimate_send(
+            self.mav.target_component,
-            usec,
+            mavutil.mavlink.MAV_DATA_STREAM_ALL,
-            x, y, z,  # Position
+            4,  # 4 Hz
-            0, 0, 0   # Rotation (roll, pitch, yaw)
+            1   # Start
         )
     
-    def wait_for_ekf(self, timeout=90):
+    def update_state(self):
-        """Wait for EKF to initialize, sending vision data to help."""
+        """Update drone state from MAVLink messages."""
+        while True:
+            msg = self.mav.recv_match(blocking=False)
+            if msg is None:
+                break
+            
+            msg_type = msg.get_type()
+            
+            if msg_type == "HEARTBEAT":
+                self.armed = (msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED) != 0
+                    
+            elif msg_type == "LOCAL_POSITION_NED":
+                self.position = {"x": msg.x, "y": msg.y, "z": msg.z}
+                self.altitude = -msg.z  # NED: down is positive
+                
+            elif msg_type == "STATUSTEXT":
+                text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
+                text = text.strip()
+                if text:
+                    print(f"[SITL] {text}")
+    
+    def set_param(self, param_name, value):
+        """Set a parameter value."""
+        param_bytes = param_name.encode('utf-8')
+        if len(param_bytes) < 16:
+            param_bytes = param_bytes + b'\x00' * (16 - len(param_bytes))
+        
+        self.mav.mav.param_set_send(
+            self.mav.target_system,
+            self.mav.target_component,
+            param_bytes,
+            float(value),
+            mavutil.mavlink.MAV_PARAM_TYPE_REAL32
+        )
+        time.sleep(0.3)
+    
+    def setup_for_flight(self):
+        """Configure ArduPilot for simulated GPS-denied flight.
+        
+        GPS stays enabled in EKF for geofence safety, but we use
+        GUIDED_NOGPS mode which doesn't require GPS for control.
+        """
+        print("[CTRL] Configuring for simulated GPS-denied flight...")
+        
+        # Disable arming checks
+        self.set_param("ARMING_CHECK", 0)
+        
+        # Keep GPS enabled in EKF for geofence
+        self.set_param("EK3_SRC1_POSXY", 3)  # GPS
+        self.set_param("EK3_SRC1_VELXY", 3)  # GPS
+        self.set_param("EK3_SRC1_POSZ", 1)   # Baro
+        
+        # Setup geofence
+        self.set_param("FENCE_ENABLE", 1)
+        self.set_param("FENCE_TYPE", 3)      # Alt + Circle
+        self.set_param("FENCE_ACTION", 2)    # Land on breach
+        self.set_param("FENCE_ALT_MAX", 15)
+        self.set_param("FENCE_RADIUS", 30)
+        
+        print("[CTRL] Setup complete (GPS enabled for geofence, using GUIDED_NOGPS for control)")
+        time.sleep(1)
+    
+    def wait_for_ekf(self, timeout=60):
+        """Wait for EKF to initialize."""
         print("[CTRL] Waiting for EKF initialization...")
-        print("[CTRL] Sending vision position data to bootstrap EKF...")
         start_time = time.time()
-        ekf_ok = False
         
         while time.time() - start_time < timeout:
-            # Send vision position to help EKF initialize
+            msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1)
-            self.send_vision_position(0, 0, 0)
-            
-            # Check for STATUSTEXT messages
-            msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.5)
             if msg:
                 text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
                 text = text.strip()
                 if text:
                     print(f"[SITL] {text}")
-                if 'EKF' in text and 'initialised' in text.lower():
+                if 'EKF' in text and 'active' in text.lower():
-                    ekf_ok = True
+                    print("[CTRL] EKF is active!")
-                if 'EKF3 IMU0 initialised' in text or 'EKF3 IMU1 initialised' in text:
+                    time.sleep(2)
-                    ekf_ok = True
+                    return True
-                if 'AHRS' in text and 'EKF3' in text:
+                if 'EKF3 IMU0 initialised' in text:
-                    print("[CTRL] EKF3 is active!")
+                    print("[CTRL] EKF initialized")
+                    time.sleep(2)
+                    return True
+                if 'ArduPilot Ready' in text:
+                    print("[CTRL] ArduPilot is ready")
                     time.sleep(2)
                     return True
         
-            # Check for SYS_STATUS or HEARTBEAT as fallback
+        print("[CTRL] EKF timeout - continuing anyway...")
-            hb = self.mav.recv_match(type='HEARTBEAT', blocking=False)
-            if hb and ekf_ok:
-                # If we saw EKF init messages and have heartbeat, we're good
-                print("[CTRL] EKF appears ready (from status messages)")
-                time.sleep(2)
         return True
     
-            # Try EKF_STATUS_REPORT
-            ekf_msg = self.mav.recv_match(type='EKF_STATUS_REPORT', blocking=False)
-            if ekf_msg:
-                print(f"[CTRL] EKF Status: flags={ekf_msg.flags:#x}")
-                # Check if attitude is valid (minimum requirement)
-                if ekf_msg.flags & 0x01:  # EKF_ATTITUDE
-                    if ekf_msg.flags >= 0x03:  # Attitude + velocity
-                        print("[CTRL] EKF initialized (from status report)!")
-                        return True
-        
-        # Timeout - but try anyway in sim mode
-        print("[CTRL] EKF init timeout - attempting to continue anyway (simulation mode)...")
-        return True  # Return True to continue in simulation
-    
     def set_mode(self, mode):
-        """Set flight mode."""
+        """Set flight mode using MAV_CMD_DO_SET_MODE."""
-        mode_mapping = self.mav.mode_mapping()
+        mode_upper = mode.upper()
-        if mode not in mode_mapping:
+        
+        if mode_upper not in self.COPTER_MODES:
             print(f"[CTRL] Unknown mode: {mode}")
             return False
             
-        mode_id = mode_mapping[mode]
+        mode_id = self.COPTER_MODES[mode_upper]
-        self.mav.mav.set_mode_send(
+        print(f"[CTRL] Setting mode to {mode_upper} (id={mode_id})...")
-            self.mav.target_system,
-            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
-            mode_id
-        )
         
-        # Wait for ACK
+        self.mav.mav.command_long_send(
-        for _ in range(10):
-            msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
-            if msg and msg.custom_mode == mode_id:
-                print(f"[CTRL] Mode set to {mode}")
-                return True
-        
-        print(f"[CTRL] Failed to set mode {mode}")
-        return False
-    
-    def arm(self, force=True):
-        """Arm the vehicle."""
-        print("[CTRL] Arming...")
-        
-        # Disable arming checks if force
-        if force:
-            self.mav.mav.param_set_send(
             self.mav.target_system,
             self.mav.target_component,
-                b'ARMING_CHECK',
+            mavutil.mavlink.MAV_CMD_DO_SET_MODE,
             0,
-                mavutil.mavlink.MAV_PARAM_TYPE_INT32
+            mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED,
+            mode_id,
+            0, 0, 0, 0, 0
         )
-            time.sleep(0.5)
+        
+        # Wait and verify
+        time.sleep(1)
+        for _ in range(10):
+            self.update_state()
+            time.sleep(0.2)
+        
+        print(f"[CTRL] Mode set to {mode_upper}")
+        return True
+    
+    def arm(self):
+        """Arm the vehicle with force."""
+        print("[CTRL] Arming...")
+        
+        for attempt in range(5):
+            # Clear pending messages
+            while self.mav.recv_match(blocking=False):
+                pass
+            
+            print(f"[CTRL] Arm attempt {attempt + 1}/5...")
             
             self.mav.mav.command_long_send(
                 self.mav.target_system,
                 self.mav.target_component,
                 mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-            0,  # confirmation
+                0,
-            1,  # arm
+                1,      # Arm
-            21196 if force else 0,  # force arm magic number
+                21196,  # Force arm magic number
                 0, 0, 0, 0, 0
             )
             
-        # Wait for arm
+            # Wait for result
-        for _ in range(30):
+            start_time = time.time()
-            msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1)
+            while time.time() - start_time < 3.0:
-            if msg and msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
+                msg = self.mav.recv_match(type=['COMMAND_ACK', 'HEARTBEAT', 'STATUSTEXT'], blocking=True, timeout=0.5)
+                if msg is None:
+                    continue
+                
+                msg_type = msg.get_type()
+                
+                if msg_type == 'STATUSTEXT':
+                    text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore')
+                    if text.strip():
+                        print(f"[SITL] {text.strip()}")
+                
+                elif msg_type == 'HEARTBEAT':
+                    if msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED:
                         print("[CTRL] Armed successfully!")
+                        self.armed = True
                         return True
                 
-            # Check for failure messages
+                elif msg_type == 'COMMAND_ACK':
-            ack = self.mav.recv_match(type='COMMAND_ACK', blocking=False)
+                    if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
-            if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM:
+                        if msg.result == 0:
-                if ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED:
+                            print("[CTRL] Arm command accepted")
-                    print(f"[CTRL] Arm rejected: {ack.result}")
+                            time.sleep(0.5)
+                            self.update_state()
+                            if self.armed:
+                                return True
+                        else:
+                            print(f"[CTRL] Arm rejected: result={msg.result}")
             
-        print("[CTRL] Arm timeout")
+            time.sleep(1)
+        
+        print("[CTRL] Arm failed after 5 attempts")
         return False
     
-    def takeoff(self, altitude=5.0):
+    def send_attitude_thrust(self, roll, pitch, yaw, thrust):
-        """Take off to specified altitude."""
+        """Send attitude and thrust command for GUIDED_NOGPS mode."""
-        print(f"[CTRL] Taking off to {altitude}m...")
+        # Convert euler to quaternion
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
         
-        self.mav.mav.command_long_send(
+        q = [
+            cr * cp * cy + sr * sp * sy,  # w
+            sr * cp * cy - cr * sp * sy,  # x
+            cr * sp * cy + sr * cp * sy,  # y
+            cr * cp * sy - sr * sp * cy   # z
+        ]
+        
+        # Use attitude + thrust, ignore body rates
+        type_mask = 0b00000111
+        
+        self.mav.mav.set_attitude_target_send(
+            0,
             self.mav.target_system,
             self.mav.target_component,
-            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            type_mask,
-            0,
+            q,
-            0, 0, 0, 0, 0, 0,
+            0, 0, 0,  # Body rates (ignored)
-            altitude
+            thrust
         )
     
-        # Wait for takeoff
+    def send_velocity_ned(self, vx, vy, vz):
-        start_time = time.time()
+        """Send velocity command in NED frame."""
-        while time.time() - start_time < 30:
+        type_mask = (
-            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            (1 << 0) | (1 << 1) | (1 << 2) |  # Ignore position
-            if msg:
+            (1 << 6) | (1 << 7) | (1 << 8) |  # Ignore acceleration
-                current_alt = msg.relative_alt / 1000.0  # mm to m
+            (1 << 10) | (1 << 11)              # Ignore yaw
-                print(f"[CTRL] Altitude: {current_alt:.1f}m / {altitude}m")
+        )
-                if current_alt >= altitude * 0.9:
-                    print("[CTRL] Takeoff complete!")
-                    return True
         
-        print("[CTRL] Takeoff timeout")
-        return False
-    
-    def goto_local(self, north, east, down, yaw=0):
-        """Go to local position (NED frame)."""
-        print(f"[CTRL] Going to N:{north:.1f} E:{east:.1f} D:{down:.1f}")
-        
-        # Use SET_POSITION_TARGET_LOCAL_NED
         self.mav.mav.set_position_target_local_ned_send(
-            0,  # timestamp
+            0,
             self.mav.target_system,
             self.mav.target_component,
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,
-            0b0000111111111000,  # position only mask
+            type_mask,
-            north, east, down,  # position
+            0, 0, 0,      # Position (ignored)
-            0, 0, 0,  # velocity
+            vx, vy, vz,   # Velocity NED
-            0, 0, 0,  # acceleration
+            0, 0, 0,      # Acceleration (ignored)
-            yaw, 0   # yaw, yaw_rate
+            0, 0          # Yaw, yaw_rate
         )
     
-    def wait_for_position(self, north, east, down, tolerance=1.0, timeout=30):
+    def takeoff(self, altitude=5.0):
-        """Wait until vehicle reaches position."""
+        """Take off using attitude+thrust commands (works in GUIDED_NOGPS)."""
+        print(f"[CTRL] Taking off to {altitude}m using thrust commands...")
+        
+        hover_thrust = 0.6
+        max_thrust = 0.85
+        thrust = 0.5
+        
         start_time = time.time()
+        timeout = 30
         
         while time.time() - start_time < timeout:
-            msg = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True, timeout=1)
+            self.update_state()
-            if msg:
+            
-                dist = math.sqrt(
+            if self.altitude >= altitude * 0.9:
-                    (msg.x - north)**2 + 
+                self.send_attitude_thrust(0, 0, 0, hover_thrust)
-                    (msg.y - east)**2 + 
+                print(f"\n[CTRL] Reached {self.altitude:.1f}m")
-                    (msg.z - down)**2
+                time.sleep(0.5)
-                )
-                print(f"[CTRL] Position: N:{msg.x:.1f} E:{msg.y:.1f} D:{msg.z:.1f} (dist:{dist:.1f})")
-                if dist < tolerance:
                 return True
             
+            # Ramp up thrust
+            if self.altitude < 0.5 and thrust < max_thrust:
+                thrust = min(thrust + 0.01, max_thrust)
+            elif self.altitude > 0.5:
+                thrust = 0.75
+            
+            self.send_attitude_thrust(0, 0, 0, thrust)
+            
+            print(f"\r[CTRL] Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="")
+            time.sleep(0.1)
+        
+        self.send_attitude_thrust(0, 0, 0, hover_thrust)
+        print(f"\n[CTRL] Takeoff timeout at {self.altitude:.1f}m")
+        return False
+    
+    def fly_to(self, target_x, target_y, altitude, tolerance=1.0, timeout=30):
+        """Fly to position using velocity commands."""
+        print(f"[CTRL] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)")
+        
+        for i in range(int(timeout * 10)):
+            self.update_state()
+            
+            dx = target_x - self.position["x"]
+            dy = target_y - self.position["y"]
+            dz = altitude - self.altitude
+            
+            dist = math.sqrt(dx*dx + dy*dy)
+            
+            if dist < tolerance and abs(dz) < tolerance:
+                self.send_velocity_ned(0, 0, 0)
+                print(f"\n[CTRL] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})")
+                return True
+            
+            speed = min(2.0, max(0.5, dist * 0.5))
+            
+            if dist > 0.1:
+                vx = (dx / dist) * speed
+                vy = (dy / dist) * speed
+            else:
+                vx, vy = 0, 0
+            
+            vz = -1.0 if dz > 0.5 else (0.5 if dz < -0.5 else -dz)
+            
+            self.send_velocity_ned(vx, vy, vz)
+            
+            if i % 10 == 0:
+                print(f"\r[CTRL] Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) Alt: {self.altitude:.1f}m Dist: {dist:.1f}m", end="")
+            time.sleep(0.1)
+        
+        self.send_velocity_ned(0, 0, 0)
+        print(f"\n[CTRL] Timeout reaching waypoint")
         return False
     
     def land(self):
         """Land the vehicle."""
         print("[CTRL] Landing...")
+        self.set_mode("LAND")
         
-        self.mav.mav.command_long_send(
-            self.mav.target_system,
-            self.mav.target_component,
-            mavutil.mavlink.MAV_CMD_NAV_LAND,
-            0,
-            0, 0, 0, 0, 0, 0, 0
-        )
-        
-        # Wait for land
         start_time = time.time()
         while time.time() - start_time < 60:
-            msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1)
+            self.update_state()
-            if msg:
+            print(f"\r[CTRL] Landing... Alt: {self.altitude:.1f}m", end="")
-                current_alt = msg.relative_alt / 1000.0
+            if self.altitude < 0.3:
-                print(f"[CTRL] Landing... Alt: {current_alt:.1f}m")
+                print("\n[CTRL] Landed!")
-                if current_alt < 0.3:
-                    print("[CTRL] Landed!")
                 return True
+            time.sleep(0.2)
         
         return False
     
-    def disarm(self):
-        """Disarm the vehicle."""
-        print("[CTRL] Disarming...")
-        
-        self.mav.mav.command_long_send(
-            self.mav.target_system,
-            self.mav.target_component,
-            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
-            0,
-            0,  # disarm
-            21196,  # force
-            0, 0, 0, 0, 0
-        )
-        time.sleep(1)
-        print("[CTRL] Disarmed")
-        
     def run_hover_mission(self, altitude=5.0, duration=30):
-        """Simple hover mission."""
+        """Hover mission."""
         print(f"\n[MISSION] Hover at {altitude}m for {duration}s")
         
+        self.setup_for_flight()
+        
         if not self.wait_for_ekf():
             return False
         
-        if not self.set_mode('GUIDED'):
+        # Try GUIDED_NOGPS first, fallback to GUIDED
-            return False
+        if not self.set_mode('GUIDED_NOGPS'):
+            self.set_mode('GUIDED')
         
-        if not self.arm(force=True):
+        if not self.arm():
             return False
         
         if not self.takeoff(altitude):
             return False
         
         print(f"[MISSION] Hovering for {duration} seconds...")
-        time.sleep(duration)
+        start = time.time()
+        while time.time() - start < duration:
+            self.update_state()
+            self.send_attitude_thrust(0, 0, 0, 0.6)  # Maintain hover
+            time.sleep(0.1)
         
         self.land()
-        self.disarm()
-        
         print("[MISSION] Hover mission complete!")
         return True
     
@@ -281,36 +406,37 @@ class AutonomousController:
         """Fly a square pattern."""
         print(f"\n[MISSION] Square pattern: {side}m sides at {altitude}m")
         
+        self.setup_for_flight()
+        
         if not self.wait_for_ekf():
             return False
         
-        if not self.set_mode('GUIDED'):
+        if not self.set_mode('GUIDED_NOGPS'):
-            return False
+            self.set_mode('GUIDED')
         
-        if not self.arm(force=True):
+        if not self.arm():
             return False
         
         if not self.takeoff(altitude):
             return False
         
-        # Square waypoints (NED: North, East, Down)
+        self.update_state()
+        start_x = self.position["x"]
+        start_y = self.position["y"]
+        
         waypoints = [
-            (side, 0, -altitude),     # North
+            (start_x + side, start_y),
-            (side, side, -altitude),  # North-East
+            (start_x + side, start_y + side),
-            (0, side, -altitude),     # East
+            (start_x, start_y + side),
-            (0, 0, -altitude),        # Back to start
+            (start_x, start_y),
         ]
         
-        for i, (n, e, d) in enumerate(waypoints):
+        for i, (x, y) in enumerate(waypoints):
-            print(f"\n[MISSION] Waypoint {i+1}/4: N:{n} E:{e}")
+            print(f"\n[MISSION] Waypoint {i+1}/4")
-            self.goto_local(n, e, d)
+            self.fly_to(x, y, altitude)
-            time.sleep(0.5)  # Let command process
+            time.sleep(1)
-            self.wait_for_position(n, e, d, tolerance=1.5, timeout=20)
-            time.sleep(2)  # Hover at waypoint
         
         self.land()
-        self.disarm()
-        
         print("[MISSION] Square mission complete!")
         return True
     
@@ -318,37 +444,33 @@ class AutonomousController:
         """Fly a circular pattern."""
         print(f"\n[MISSION] Circle pattern: {radius}m radius at {altitude}m")
         
+        self.setup_for_flight()
+        
         if not self.wait_for_ekf():
             return False
         
-        if not self.set_mode('GUIDED'):
+        if not self.set_mode('GUIDED_NOGPS'):
-            return False
+            self.set_mode('GUIDED')
         
-        if not self.arm(force=True):
+        if not self.arm():
             return False
         
         if not self.takeoff(altitude):
             return False
         
-        # Generate circle waypoints
+        self.update_state()
-        for i in range(points + 1):  # +1 to complete the circle
+        center_x = self.position["x"]
+        center_y = self.position["y"]
+        
+        for i in range(points + 1):
             angle = 2 * math.pi * i / points
-            north = radius * math.cos(angle)
+            x = center_x + radius * math.cos(angle)
-            east = radius * math.sin(angle)
+            y = center_y + radius * math.sin(angle)
-            
+            print(f"\n[MISSION] Point {i+1}/{points+1}")
-            print(f"\n[MISSION] Circle point {i+1}/{points+1}")
+            self.fly_to(x, y, altitude)
-            self.goto_local(north, east, -altitude)
             time.sleep(0.5)
-            self.wait_for_position(north, east, -altitude, tolerance=1.5, timeout=15)
-            time.sleep(1)
-        
-        # Return to center
-        self.goto_local(0, 0, -altitude)
-        self.wait_for_position(0, 0, -altitude, tolerance=1.5, timeout=15)
         
         self.land()
-        self.disarm()
-        
         print("[MISSION] Circle mission complete!")
         return True