#!/usr/bin/env python3 """ Autonomous UAV Controller ========================= GPS-denied navigation simulation with GPS-based geofencing for safety. Uses GUIDED_NOGPS mode with velocity/attitude commands while keeping GPS enabled in EKF for geofence and LOCAL_POSITION_NED telemetry. Usage: python src/autonomous_controller.py --mission hover python src/autonomous_controller.py --mission square --size 5 python src/autonomous_controller.py --mission circle --size 3 """ import sys import os import time import math import argparse 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 controller.""" self.connection_string = connection_string self.mav = None self.armed = False self.mode = "UNKNOWN" self.altitude = 0 self.position = {"x": 0, "y": 0, "z": 0} def connect(self, timeout=30): """Connect to ArduPilot SITL.""" print(f"[CTRL] Connecting to {self.connection_string}...") try: self.mav = mavutil.mavlink_connection(self.connection_string) print("[CTRL] Waiting for heartbeat...") msg = None for attempt in range(3): msg = self.mav.wait_heartbeat(timeout=timeout) if msg and self.mav.target_system > 0: break print(f"[CTRL] Heartbeat attempt {attempt + 1}/3...") if not msg or self.mav.target_system == 0: print("[ERROR] Failed to receive heartbeat") return False print(f"[CTRL] Connected! System {self.mav.target_system}, Component {self.mav.target_component}") # Request ALL data streams at higher rate self.mav.mav.request_data_stream_send( self.mav.target_system, self.mav.target_component, mavutil.mavlink.MAV_DATA_STREAM_ALL, 50, # 50 Hz for everything 1 # Start ) # Specifically request position stream at high rate self.mav.mav.request_data_stream_send( self.mav.target_system, self.mav.target_component, mavutil.mavlink.MAV_DATA_STREAM_POSITION, 50, # 50 Hz 1 ) # Request extended status for altitude self.mav.mav.request_data_stream_send( self.mav.target_system, self.mav.target_component, mavutil.mavlink.MAV_DATA_STREAM_EXTENDED_STATUS, 10, 1 ) return True except Exception as e: print(f"[ERROR] Connection failed: {e}") return False 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 try: self.mode = mavutil.mode_string_v10(msg) except: self.mode = str(msg.custom_mode) 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 == "GLOBAL_POSITION_INT": # Backup altitude source (relative altitude in mm) if self.altitude == 0: self.altitude = msg.relative_alt / 1000.0 elif msg_type == "VFR_HUD": # Another backup altitude source if self.altitude == 0: self.altitude = msg.alt 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.5) def setup_gps_denied(self): """Configure for simulated GPS-denied operation. GPS is enabled in EKF for: 1. Geofence safety (enabled after GPS lock) 2. LOCAL_POSITION_NED telemetry But we use GUIDED_NOGPS mode and send velocity/attitude commands, simulating GPS-denied flight control. """ print("[CTRL] Configuring for GPS-denied simulation...") # Disable arming checks self.set_param("ARMING_CHECK", 0) # Enable GPS in EKF (for geofence and position telemetry) self.set_param("EK3_SRC1_POSXY", 3) # GPS self.set_param("EK3_SRC1_VELXY", 3) # GPS self.set_param("EK3_SRC1_POSZ", 1) # Baro # Disable GPS failsafe (we're simulating GPS-denied) self.set_param("FS_GPS_ENABLE", 0) # DISABLE fence initially - will enable after GPS lock self.set_param("FENCE_ENABLE", 0) print("[CTRL] Setup complete (fence will enable after GPS lock)") time.sleep(1) def wait_for_ready(self, timeout=90): """Wait for GPS lock and home position.""" print("[CTRL] Waiting for GPS lock and home position...") start = time.time() gps_ok = False home_ok = False while time.time() - start < timeout: self.update_state() # Check multiple message types msg = self.mav.recv_match(type=['STATUSTEXT', 'GPS_RAW_INT', 'HOME_POSITION'], blocking=True, timeout=1) if msg: 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') text = text.strip() if text: print(f"[SITL] {text}") # Check for origin set (means home is set) if 'origin set' in text.lower() or 'Field Elevation' in text: home_ok = True print("[CTRL] Home position set!") elif msg_type == 'GPS_RAW_INT': if msg.fix_type >= 3: # 3D fix if not gps_ok: print(f"[CTRL] GPS fix: {msg.fix_type} (satellites: {msg.satellites_visible})") gps_ok = True elif msg_type == 'HOME_POSITION': home_ok = True print("[CTRL] Home position received!") # Check if ready if gps_ok and home_ok: print("[CTRL] GPS and home ready!") # Wait for EKF position estimate to be valid print("[CTRL] Waiting for EKF position estimate...") time.sleep(5) # Skip fence for now - causes "need position estimate" errors # TODO: Re-enable fence after basic flight works # print("[CTRL] Enabling geofence: Alt=10m, Radius=20m") # self.set_param("FENCE_ENABLE", 1) # self.set_param("FENCE_TYPE", 3) # self.set_param("FENCE_ACTION", 2) # self.set_param("FENCE_ALT_MAX", 10) # self.set_param("FENCE_RADIUS", 20) return True elapsed = int(time.time() - start) if elapsed % 10 == 0 and elapsed > 0: status = f"GPS={'OK' if gps_ok else 'waiting'}, Home={'OK' if home_ok else 'waiting'}" print(f"[CTRL] Waiting... ({elapsed}s) {status}") print("[CTRL] Ready timeout - continuing anyway (fence disabled)") return True def set_mode(self, mode_name): """Set flight mode.""" mode_upper = mode_name.upper() if mode_upper not in self.COPTER_MODES: print(f"[CTRL] Unknown mode: {mode_name}") return False mode_id = self.COPTER_MODES[mode_upper] print(f"[CTRL] Setting mode to {mode_upper}...") self.mav.mav.command_long_send( self.mav.target_system, self.mav.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, mavutil.mavlink.MAV_MODE_FLAG_CUSTOM_MODE_ENABLED, mode_id, 0, 0, 0, 0, 0 ) time.sleep(1) for _ in range(10): self.update_state() if mode_upper in self.mode.upper(): print(f"[CTRL] Mode: {self.mode}") return True time.sleep(0.2) print(f"[CTRL] Mode set (current: {self.mode})") return True def arm(self): """Force arm the drone.""" print("[CTRL] Arming...") for attempt in range(5): 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 0, 0, 0, 0, 0 ) start = time.time() while time.time() - start < 3.0: 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 elif msg_type == 'COMMAND_ACK': if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: if msg.result == 0: print("[CTRL] Arm accepted") time.sleep(0.5) self.update_state() if self.armed: return True time.sleep(1) print("[CTRL] Arm failed") return False def send_attitude_thrust(self, roll, pitch, yaw, thrust): """Send attitude and thrust command for GUIDED_NOGPS mode.""" 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) q = [ cr * cp * cy + sr * sp * sy, sr * cp * cy - cr * sp * sy, cr * sp * cy + sr * cp * sy, cr * cp * sy - sr * sp * cy ] type_mask = 0b00000111 # Use attitude + thrust self.mav.mav.set_attitude_target_send( 0, self.mav.target_system, self.mav.target_component, type_mask, q, 0, 0, 0, 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 ) self.mav.mav.set_position_target_local_ned_send( 0, self.mav.target_system, self.mav.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, type_mask, 0, 0, 0, vx, vy, vz, 0, 0, 0, 0, 0 ) def takeoff(self, altitude=5.0): """Take off using thrust commands.""" print(f"[CTRL] Taking off to {altitude}m...") hover_thrust = 0.6 max_thrust = 0.85 thrust = 0.5 start = time.time() timeout = 30 last_alt = 0 stuck_count = 0 got_alt_reading = False while time.time() - start < timeout: # Actively poll for position messages for _ in range(5): msg = self.mav.recv_match(type=['LOCAL_POSITION_NED', 'GLOBAL_POSITION_INT', 'VFR_HUD'], blocking=True, timeout=0.02) if msg: msg_type = msg.get_type() if msg_type == 'LOCAL_POSITION_NED': self.altitude = -msg.z self.position = {"x": msg.x, "y": msg.y, "z": msg.z} got_alt_reading = True elif msg_type == 'GLOBAL_POSITION_INT': self.altitude = msg.relative_alt / 1000.0 got_alt_reading = True elif msg_type == 'VFR_HUD': # VFR_HUD.alt is MSL, but climb rate is useful if msg.climb > 0.1: got_alt_reading = True self.update_state() if self.altitude >= altitude * 0.90: 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 thrust more aggressively elapsed = time.time() - start if elapsed < 2.0: # Initial ramp thrust = 0.5 + (elapsed / 2.0) * 0.35 # 0.5 -> 0.85 in 2 seconds elif self.altitude > 0.5: # Flying - reduce to climb thrust thrust = 0.75 else: # Max thrust if not climbing after 2s thrust = max_thrust # Only check for stuck if we have altitude readings if got_alt_reading and elapsed > 5.0: if abs(self.altitude - last_alt) < 0.01: stuck_count += 1 if stuck_count > 50: # 5 seconds stuck print(f"\n[WARN] Not climbing at thrust={thrust:.2f}, alt={self.altitude:.2f}m") break else: stuck_count = 0 last_alt = self.altitude self.send_attitude_thrust(0, 0, 0, thrust) alt_str = f"{self.altitude:.1f}" if got_alt_reading else "?" print(f"\r[CTRL] Climbing: {alt_str}m / {altitude:.1f}m (thrust={thrust:.2f}, t={elapsed:.1f}s)", end="") time.sleep(0.1) self.send_attitude_thrust(0, 0, 0, hover_thrust) print(f"\n[CTRL] Takeoff ended at {self.altitude:.1f}m") return self.altitude > 1.0 def fly_to(self, target_x, target_y, altitude, tolerance=1.0, timeout=60): """Fly to position using attitude+thrust commands (works in GUIDED_NOGPS). Uses pitch for forward/backward movement, roll for left/right. """ print(f"[CTRL] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)") hover_thrust = 0.6 max_tilt = 0.15 # ~8.5 degrees max tilt angle in radians start_time = time.time() while time.time() - start_time < timeout: # Actively poll for position for _ in range(3): msg = self.mav.recv_match(type=['LOCAL_POSITION_NED', 'GLOBAL_POSITION_INT'], blocking=True, timeout=0.03) if msg: if msg.get_type() == 'LOCAL_POSITION_NED': self.altitude = -msg.z self.position = {"x": msg.x, "y": msg.y, "z": msg.z} elif msg.get_type() == 'GLOBAL_POSITION_INT': self.altitude = msg.relative_alt / 1000.0 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) # Check if reached target if dist < tolerance and abs(dz) < tolerance: self.send_attitude_thrust(0, 0, 0, hover_thrust) print(f"\n[CTRL] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})") return True # Calculate tilt angles for movement # In NED frame: +X is forward (North), +Y is right (East) # Pitch: negative = nose down = fly forward (+X) # Roll: positive = right wing down = fly right (+Y) if dist > 0.1: # Calculate direction to target # Scale tilt based on distance (more tilt = faster movement) tilt_scale = min(1.0, dist / 5.0) # Full tilt at 5+ meters away # Pitch for forward/back movement (negative pitch = forward) pitch = -max_tilt * tilt_scale * (dx / dist) # Roll for left/right movement (positive roll = right) roll = max_tilt * tilt_scale * (dy / dist) else: pitch = 0 roll = 0 # Thrust for altitude if dz > 0.5: thrust = hover_thrust + 0.1 # Climb elif dz < -0.5: thrust = hover_thrust - 0.1 # Descend else: thrust = hover_thrust + dz * 0.1 # Small adjustment # Clamp thrust thrust = max(0.4, min(0.8, thrust)) self.send_attitude_thrust(roll, pitch, 0, thrust) elapsed = time.time() - start_time if int(elapsed * 10) % 10 == 0: # Print every second print(f"\r[CTRL] Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) -> ({target_x:.1f}, {target_y:.1f}) Dist: {dist:.1f}m Alt: {self.altitude:.1f}m", end="", flush=True) time.sleep(0.05) # 20Hz control rate self.send_attitude_thrust(0, 0, 0, hover_thrust) print(f"\n[CTRL] Waypoint timeout") return False def land(self): """Land the drone.""" print("[CTRL] Landing...") self.set_mode("LAND") for _ in range(100): 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 # ==================== MISSIONS ==================== def run_hover_mission(self, altitude=5.0, duration=30): """Hover in place.""" print(f"\n{'='*50}") print(f" HOVER MISSION: {altitude}m for {duration}s") print(f"{'='*50}\n") self.setup_gps_denied() self.wait_for_ready() if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') if not self.arm(): return False if not self.takeoff(altitude): self.land() return False print(f"[CTRL] Hovering for {duration}s...") start = time.time() while time.time() - start < duration: self.update_state() self.send_attitude_thrust(0, 0, 0, 0.6) remaining = duration - (time.time() - start) print(f"\r[CTRL] Hovering... {remaining:.0f}s remaining, Alt: {self.altitude:.1f}m ", end="", flush=True) time.sleep(0.1) print() self.land() print("\n[CTRL] Hover mission complete!") return True def run_square_mission(self, altitude=5.0, size=5.0): """Fly a square pattern.""" print(f"\n{'='*50}") print(f" SQUARE MISSION: {size}m x {size}m at {altitude}m") print(f"{'='*50}\n") self.setup_gps_denied() self.wait_for_ready() if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') if not self.arm(): return False if not self.takeoff(altitude): self.land() return False self.update_state() start_x = self.position["x"] start_y = self.position["y"] waypoints = [ (start_x + size, start_y), (start_x + size, start_y + size), (start_x, start_y + size), (start_x, start_y), ] print(f"[CTRL] Flying square from ({start_x:.1f}, {start_y:.1f})") for i, (x, y) in enumerate(waypoints): print(f"\n--- Waypoint {i+1}/4 ---") self.fly_to(x, y, altitude) time.sleep(1) self.land() print("\n[CTRL] Square mission complete!") return True def run_circle_mission(self, altitude=5.0, radius=5.0, points=8): """Fly a circular pattern.""" print(f"\n{'='*50}") print(f" CIRCLE MISSION: radius={radius}m at {altitude}m") print(f"{'='*50}\n") self.setup_gps_denied() self.wait_for_ready() if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') if not self.arm(): return False if not self.takeoff(altitude): self.land() return False self.update_state() center_x = self.position["x"] center_y = self.position["y"] print(f"[CTRL] Flying circle around ({center_x:.1f}, {center_y:.1f})") 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--- Point {i+1}/{points+1} ---") self.fly_to(x, y, altitude) time.sleep(0.5) self.land() print("\n[CTRL] Circle mission complete!") return True def main(): parser = argparse.ArgumentParser(description='Autonomous UAV Controller') parser.add_argument('--connection', '-c', default='tcp:127.0.0.1:5760', help='MAVLink connection string') parser.add_argument('--mission', '-m', choices=['hover', 'square', 'circle'], default='hover', help='Mission type') parser.add_argument('--altitude', '-a', type=float, default=5.0, help='Flight altitude (meters)') parser.add_argument('--size', '-s', type=float, default=5.0, help='Pattern size/radius (meters)') parser.add_argument('--duration', '-d', type=float, default=30.0, help='Hover duration (seconds)') args = parser.parse_args() print("=" * 50) print(" Autonomous UAV Controller") print(" GPS-Denied Navigation Simulation") print("=" * 50) print(f" Connection: {args.connection}") print(f" Mission: {args.mission}") print(f" Altitude: {args.altitude}m") print("=" * 50) controller = AutonomousController(args.connection) if not controller.connect(): print("\n[ERROR] Could not connect to SITL") sys.exit(1) try: if args.mission == 'hover': controller.run_hover_mission(args.altitude, args.duration) elif args.mission == 'square': controller.run_square_mission(args.altitude, args.size) elif args.mission == 'circle': controller.run_circle_mission(args.altitude, args.size) except KeyboardInterrupt: print("\n\n[CTRL] Interrupted - Landing...") controller.land() except Exception as e: print(f"\n[ERROR] {e}") import traceback traceback.print_exc() controller.land() if __name__ == '__main__': main()