diff --git a/scripts/run_autonomous.sh b/scripts/run_autonomous.sh index 9b21b68..5587efe 100755 --- a/scripts/run_autonomous.sh +++ b/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 World file to load (default: iris_runway.sdf)" echo " --mission 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 \ - --add-param-file "$PARAM_FILE" & -else - sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --no-mavproxy -I0 & + SITL_ARGS="$SITL_ARGS --add-param-file $PARAM_FILE" fi + +# Start SITL +sim_vehicle.py $SITL_ARGS & SITL_PID=$! # Wait longer for SITL to fully initialize diff --git a/src/autonomous_controller.py b/src/autonomous_controller.py index 1507e8b..11e6990 100755 --- a/src/autonomous_controller.py +++ b/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): - """Send vision position estimate to help EKF initialize.""" - usec = int(time.time() * 1e6) - self.mav.mav.vision_position_estimate_send( - usec, - x, y, z, # Position - 0, 0, 0 # Rotation (roll, pitch, yaw) + # Request data streams + self.mav.mav.request_data_stream_send( + self.mav.target_system, + self.mav.target_component, + mavutil.mavlink.MAV_DATA_STREAM_ALL, + 4, # 4 Hz + 1 # Start ) - def wait_for_ekf(self, timeout=90): - """Wait for EKF to initialize, sending vision data to help.""" + def update_state(self): + """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 - self.send_vision_position(0, 0, 0) - - # Check for STATUSTEXT messages - msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=0.5) + msg = self.mav.recv_match(type='STATUSTEXT', blocking=True, timeout=1) 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(): - ekf_ok = True - if 'EKF3 IMU0 initialised' in text or 'EKF3 IMU1 initialised' in text: - ekf_ok = True - if 'AHRS' in text and 'EKF3' in text: - print("[CTRL] EKF3 is active!") + if 'EKF' in text and 'active' in text.lower(): + print("[CTRL] EKF is active!") + time.sleep(2) + return True + if 'EKF3 IMU0 initialised' in text: + 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 - 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 + print("[CTRL] EKF timeout - continuing anyway...") + return True def set_mode(self, mode): - """Set flight mode.""" - mode_mapping = self.mav.mode_mapping() - if mode not in mode_mapping: + """Set flight mode using MAV_CMD_DO_SET_MODE.""" + mode_upper = mode.upper() + + if mode_upper not in self.COPTER_MODES: print(f"[CTRL] Unknown mode: {mode}") return False - mode_id = mode_mapping[mode] - self.mav.mav.set_mode_send( - self.mav.target_system, - mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, - mode_id - ) - - # Wait for ACK - 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', - 0, - mavutil.mavlink.MAV_PARAM_TYPE_INT32 - ) - time.sleep(0.5) + mode_id = self.COPTER_MODES[mode_upper] + print(f"[CTRL] Setting mode to {mode_upper} (id={mode_id})...") self.mav.mav.command_long_send( self.mav.target_system, self.mav.target_component, - mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM, - 0, # confirmation - 1, # arm - 21196 if force else 0, # force arm magic number + mavutil.mavlink.MAV_CMD_DO_SET_MODE, + 0, + mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, + mode_id, 0, 0, 0, 0, 0 ) - # Wait for arm - for _ in range(30): - msg = self.mav.recv_match(type='HEARTBEAT', blocking=True, timeout=1) - if msg and msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED: - print("[CTRL] Armed successfully!") - return True + # 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, + 1, # Arm + 21196, # Force arm magic number + 0, 0, 0, 0, 0 + ) + + # Wait for result + start_time = time.time() + while time.time() - start_time < 3.0: + msg = self.mav.recv_match(type=['COMMAND_ACK', 'HEARTBEAT', 'STATUSTEXT'], blocking=True, timeout=0.5) + if msg is None: + continue - # Check for failure messages - ack = self.mav.recv_match(type='COMMAND_ACK', blocking=False) - if ack and ack.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: - if ack.result != mavutil.mavlink.MAV_RESULT_ACCEPTED: - print(f"[CTRL] Arm rejected: {ack.result}") - - print("[CTRL] Arm timeout") + 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 + + elif msg_type == 'COMMAND_ACK': + if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: + if msg.result == 0: + print("[CTRL] Arm command accepted") + time.sleep(0.5) + self.update_state() + if self.armed: + return True + else: + print(f"[CTRL] Arm rejected: result={msg.result}") + + time.sleep(1) + + print("[CTRL] Arm failed after 5 attempts") return False - def takeoff(self, altitude=5.0): - """Take off to specified altitude.""" - print(f"[CTRL] Taking off to {altitude}m...") + def send_attitude_thrust(self, roll, pitch, yaw, thrust): + """Send attitude and thrust command for GUIDED_NOGPS mode.""" + # 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, - 0, - 0, 0, 0, 0, 0, 0, - altitude + type_mask, + q, + 0, 0, 0, # Body rates (ignored) + thrust + ) + + def send_velocity_ned(self, vx, vy, vz): + """Send velocity command in NED frame.""" + type_mask = ( + (1 << 0) | (1 << 1) | (1 << 2) | # Ignore position + (1 << 6) | (1 << 7) | (1 << 8) | # Ignore acceleration + (1 << 10) | (1 << 11) # Ignore yaw ) - # Wait for takeoff - start_time = time.time() - while time.time() - start_time < 30: - msg = self.mav.recv_match(type='GLOBAL_POSITION_INT', blocking=True, timeout=1) - if msg: - current_alt = msg.relative_alt / 1000.0 # mm to m - 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 - north, east, down, # position - 0, 0, 0, # velocity - 0, 0, 0, # acceleration - yaw, 0 # yaw, yaw_rate + type_mask, + 0, 0, 0, # Position (ignored) + vx, vy, vz, # Velocity NED + 0, 0, 0, # Acceleration (ignored) + 0, 0 # Yaw, yaw_rate ) + + def takeoff(self, altitude=5.0): + """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 - def wait_for_position(self, north, east, down, tolerance=1.0, timeout=30): - """Wait until vehicle reaches position.""" 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) - if msg: - dist = math.sqrt( - (msg.x - north)**2 + - (msg.y - east)**2 + - (msg.z - down)**2 - ) - print(f"[CTRL] Position: N:{msg.x:.1f} E:{msg.y:.1f} D:{msg.z:.1f} (dist:{dist:.1f})") - if dist < tolerance: - return True - + self.update_state() + + if self.altitude >= altitude * 0.9: + self.send_attitude_thrust(0, 0, 0, hover_thrust) + print(f"\n[CTRL] Reached {self.altitude:.1f}m") + time.sleep(0.5) + 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) - if msg: - current_alt = msg.relative_alt / 1000.0 - print(f"[CTRL] Landing... Alt: {current_alt:.1f}m") - if current_alt < 0.3: - print("[CTRL] Landed!") - return True - + self.update_state() + print(f"\r[CTRL] Landing... Alt: {self.altitude:.1f}m", end="") + if self.altitude < 0.3: + print("\n[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 + if not self.set_mode('GUIDED_NOGPS'): + self.set_mode('GUIDED') + + if not self.arm(): return False - - if not self.arm(force=True): - 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'): + self.set_mode('GUIDED') + + if not self.arm(): return False - - if not self.arm(force=True): - 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 - (side, side, -altitude), # North-East - (0, side, -altitude), # East - (0, 0, -altitude), # Back to start + (start_x + side, start_y), + (start_x + side, start_y + side), + (start_x, start_y + side), + (start_x, start_y), ] - for i, (n, e, d) in enumerate(waypoints): - print(f"\n[MISSION] Waypoint {i+1}/4: N:{n} E:{e}") - self.goto_local(n, e, d) - time.sleep(0.5) # Let command process - self.wait_for_position(n, e, d, tolerance=1.5, timeout=20) - time.sleep(2) # Hover at waypoint + for i, (x, y) in enumerate(waypoints): + print(f"\n[MISSION] Waypoint {i+1}/4") + self.fly_to(x, y, altitude) + time.sleep(1) 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'): + self.set_mode('GUIDED') + + if not self.arm(): return False - - if not self.arm(force=True): - return False - + if not self.takeoff(altitude): return False - # Generate circle waypoints - for i in range(points + 1): # +1 to complete the circle - angle = 2 * math.pi * i / points - north = radius * math.cos(angle) - east = radius * math.sin(angle) - - print(f"\n[MISSION] Circle point {i+1}/{points+1}") - 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) + self.update_state() + center_x = self.position["x"] + center_y = self.position["y"] - # Return to center - self.goto_local(0, 0, -altitude) - self.wait_for_position(0, 0, -altitude, tolerance=1.5, timeout=15) + for i in range(points + 1): + angle = 2 * math.pi * i / points + x = center_x + radius * math.cos(angle) + y = center_y + radius * math.sin(angle) + print(f"\n[MISSION] Point {i+1}/{points+1}") + self.fly_to(x, y, altitude) + time.sleep(0.5) self.land() - self.disarm() - print("[MISSION] Circle mission complete!") return True