diff --git a/config/ardupilot_gps_denied.parm b/config/ardupilot_gps_denied.parm index a2e1019..d1a7237 100644 --- a/config/ardupilot_gps_denied.parm +++ b/config/ardupilot_gps_denied.parm @@ -1,73 +1,46 @@ -# ArduPilot Parameters for Simulation -# Uses GPS from Gazebo for position estimation +# ArduPilot Parameters for GPS-Denied Navigation Simulation +# ========================================================= +# GPS is ENABLED for: Geofence safety, LOCAL_POSITION_NED telemetry +# GPS is SIMULATED DENIED for: Control (using GUIDED_NOGPS mode) # ==================== # GPS Configuration # ==================== -# Enable GPS (Gazebo provides simulated GPS) +# Enable GPS (needed for geofence and position telemetry) GPS_TYPE 1 # Auto-detect -GPS_TYPE2 0 # ==================== # Geofence Settings # ==================== -# Disable geofence for simulation (avoids position requirement issues) -FENCE_ENABLE 0 +# Safety cage - uses GPS for boundaries +FENCE_ENABLE 1 FENCE_TYPE 3 # Alt + Circle FENCE_ACTION 2 # Land on breach -FENCE_ALT_MAX 50 # Maximum altitude (meters) -FENCE_RADIUS 100 # Circular fence radius (meters) +FENCE_ALT_MAX 10 # Maximum altitude (meters) +FENCE_RADIUS 20 # Circular fence radius (meters) FENCE_MARGIN 2 # Margin inside fence for warning # ==================== # EKF Configuration # ==================== -# Use EKF3 with GPS +# Use EKF3 with GPS (for telemetry) AHRS_EKF_TYPE 3 EK3_ENABLE 1 EK2_ENABLE 0 -# EKF3 Source Configuration - Use GPS +# EKF3 Source - GPS for position (enables LOCAL_POSITION_NED) EK3_SRC1_POSXY 3 # GPS for XY position EK3_SRC1_POSZ 1 # Barometer for Z position EK3_SRC1_VELXY 3 # GPS for XY velocity EK3_SRC1_VELZ 0 # None for Z velocity EK3_SRC1_YAW 1 # Compass for yaw -# EKF3 Position Innovation Gate -EK3_POS_I_GATE 300 # Larger gate - -# ==================== -# Vision Position Input -# ==================== -# Disable vision odometry (not using for simulation) -VISO_TYPE 0 # Disabled -VISO_POS_X 0.0 -VISO_POS_Y 0.0 -VISO_POS_Z 0.0 -VISO_ORIENT 0 -VISO_DELAY_MS 50 - - -# ==================== -# Optical Flow -# ==================== -# Disable optical flow (using GPS instead) -FLOW_TYPE 0 # Disabled - -# ==================== -# Rangefinder -# ==================== -# Disable rangefinder (using barometer + GPS) -RNGFND1_TYPE 0 # Disabled - # ==================== # Failsafe Settings # ==================== FS_EKF_ACTION 1 # Land on EKF failsafe FS_EKF_THRESH 0.8 # EKF failsafe threshold -FS_VIBE_ENABLE 0 # Disable vibration failsafe -FS_GPS_ENABLE 0 # Disable GPS failsafe for simulation +FS_GPS_ENABLE 0 # Disable GPS failsafe (we simulate GPS-denied) # ==================== # Flight Modes @@ -82,12 +55,10 @@ FLTMODE6 9 # Land # ==================== # Arming Checks # ==================== -# Disable arming checks for simulation testing -ARMING_CHECK 0 # Disable all checks (for sim only) +ARMING_CHECK 0 # Disable all checks (for simulation) # ==================== # Logging # ==================== LOG_BITMASK 176126 # Log all relevant data LOG_DISARMED 0 # Don't log when disarmed - diff --git a/src/autonomous_controller.py b/src/autonomous_controller.py index 89af0b9..ca2c0df 100755 --- a/src/autonomous_controller.py +++ b/src/autonomous_controller.py @@ -1,16 +1,24 @@ #!/usr/bin/env python3 """ -Autonomous UAV Controller using pymavlink. -Connects directly to ArduPilot SITL and controls the drone autonomously. -No ROS/MAVROS required. +Autonomous UAV Controller +========================= +GPS-denied navigation simulation with GPS-based geofencing for safety. -Uses GUIDED_NOGPS mode for GPS-denied flight simulation while keeping -GPS enabled in EKF for geofence 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 @@ -32,28 +40,48 @@ class AutonomousController: } def __init__(self, connection_string='tcp:127.0.0.1:5760'): - """Initialize connection to ArduPilot.""" - print(f"[CTRL] Connecting to {connection_string}...") - self.mav = mavutil.mavlink_connection(connection_string) - - # Wait for heartbeat - print("[CTRL] Waiting for heartbeat...") - self.mav.wait_heartbeat() - print(f"[CTRL] Connected! System {self.mav.target_system} Component {self.mav.target_component}") - - self.home_position = None + """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} - self.armed = False - # 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 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 data streams for telemetry + self.mav.mav.request_data_stream_send( + self.mav.target_system, + self.mav.target_component, + mavutil.mavlink.MAV_DATA_STREAM_ALL, + 10, # 10 Hz + 1 # Start + ) + + return True + except Exception as e: + print(f"[ERROR] Connection failed: {e}") + return False def update_state(self): """Update drone state from MAVLink messages.""" @@ -66,6 +94,10 @@ class AutonomousController: 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} @@ -90,105 +122,85 @@ class AutonomousController: float(value), mavutil.mavlink.MAV_PARAM_TYPE_REAL32 ) - time.sleep(0.3) + time.sleep(0.5) - def setup_for_flight(self): - """Configure ArduPilot for simulation flight. + def setup_gps_denied(self): + """Configure for simulated GPS-denied operation. - For Gazebo simulation, we use GPS (provided by Gazebo) for position. - We disable the fence to avoid position requirement conflicts. + GPS is enabled in EKF for: + 1. Geofence safety + 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 simulation...") + print("[CTRL] Configuring for GPS-denied simulation...") - # Disable ALL arming checks for simulation + # Disable arming checks self.set_param("ARMING_CHECK", 0) - # Enable GPS (Gazebo provides simulated GPS) - self.set_param("GPS_TYPE", 1) # Auto - - # Configure EKF to use GPS + # 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 fence to avoid "fence requires position" error - self.set_param("FENCE_ENABLE", 0) - - # Disable vision odometry requirement - self.set_param("VISO_TYPE", 0) - - # Disable GPS failsafe + # Disable GPS failsafe (we're simulating GPS-denied) self.set_param("FS_GPS_ENABLE", 0) - print("[CTRL] Setup complete") - time.sleep(1) + # Setup geofence (safety cage) + print("[CTRL] Setting up geofence: Alt=10m, Radius=20m") + 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", 10) + self.set_param("FENCE_RADIUS", 20) + self.set_param("FENCE_MARGIN", 2.0) - # Wait for GPS lock and home position - print("[CTRL] Waiting for GPS lock and home position...") - for i in range(60): # 60 second timeout + print("[CTRL] Setup complete (GPS for fence, GUIDED_NOGPS for control)") + time.sleep(1) + + def wait_for_ready(self, timeout=60): + """Wait for EKF and GPS to be ready.""" + print("[CTRL] Waiting for system ready...") + start = time.time() + + while time.time() - start < timeout: self.update_state() - # Check for GPS_RAW_INT message - msg = self.mav.recv_match(type=['GPS_RAW_INT', 'HOME_POSITION', 'STATUSTEXT'], blocking=True, timeout=1) + msg = self.mav.recv_match(type=['STATUSTEXT', 'GPS_RAW_INT'], blocking=True, timeout=1) if msg: msg_type = msg.get_type() - if msg_type == 'GPS_RAW_INT': - fix = msg.fix_type - if fix >= 3: # 3D fix - print(f"[CTRL] GPS fix: {fix} (satellites: {msg.satellites_visible})") - return True - elif msg_type == 'HOME_POSITION': - print("[CTRL] Home position set") - return True - elif msg_type == 'STATUSTEXT': + 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()}") + text = text.strip() + if text: + print(f"[SITL] {text}") + if 'EKF3' in text and 'active' in text.lower(): + print("[CTRL] EKF ready!") + time.sleep(2) + return True + elif msg_type == 'GPS_RAW_INT': + if msg.fix_type >= 3: + print(f"[CTRL] GPS fix: {msg.fix_type} (satellites: {msg.satellites_visible})") + time.sleep(1) + return True - if i % 10 == 0: - print(f"[CTRL] Waiting for GPS... ({i}s)") + if int(time.time() - start) % 10 == 0 and int(time.time() - start) > 0: + print(f"[CTRL] Waiting... ({int(time.time() - start)}s)") - print("[CTRL] GPS timeout - continuing anyway") + print("[CTRL] Ready timeout - continuing anyway") return True - def wait_for_ekf(self, timeout=60): - """Wait for EKF to initialize.""" - print("[CTRL] Waiting for EKF initialization...") - start_time = time.time() - - while time.time() - start_time < timeout: - 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 '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 - - print("[CTRL] EKF timeout - continuing anyway...") - return True - - def set_mode(self, mode): - """Set flight mode using MAV_CMD_DO_SET_MODE.""" - mode_upper = mode.upper() + 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}") + print(f"[CTRL] Unknown mode: {mode_name}") return False mode_id = self.COPTER_MODES[mode_upper] - print(f"[CTRL] Setting mode to {mode_upper} (id={mode_id})...") + print(f"[CTRL] Setting mode to {mode_upper}...") self.mav.mav.command_long_send( self.mav.target_system, @@ -200,39 +212,38 @@ class AutonomousController: 0, 0, 0, 0, 0 ) - # Wait and verify 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 to {mode_upper}") + print(f"[CTRL] Mode set (current: {self.mode})") return True def arm(self): - """Arm the vehicle with force.""" + """Force arm the drone.""" 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 + 21196, # Force arm 0, 0, 0, 0, 0 ) - # Wait for result - start_time = time.time() - while time.time() - start_time < 3.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 @@ -253,22 +264,19 @@ class AutonomousController: elif msg_type == 'COMMAND_ACK': if msg.command == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM: if msg.result == 0: - print("[CTRL] Arm command accepted") + print("[CTRL] Arm 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") + print("[CTRL] Arm failed") return False 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) @@ -277,14 +285,13 @@ class AutonomousController: sr = math.sin(roll * 0.5) 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 + cr * cp * cy + sr * sp * sy, + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy ] - # Use attitude + thrust, ignore body rates - type_mask = 0b00000111 + type_mask = 0b00000111 # Use attitude + thrust self.mav.mav.set_attitude_target_send( 0, @@ -292,7 +299,7 @@ class AutonomousController: self.mav.target_component, type_mask, q, - 0, 0, 0, # Body rates (ignored) + 0, 0, 0, thrust ) @@ -310,46 +317,58 @@ class AutonomousController: self.mav.target_component, mavutil.mavlink.MAV_FRAME_LOCAL_NED, type_mask, - 0, 0, 0, # Position (ignored) - vx, vy, vz, # Velocity NED - 0, 0, 0, # Acceleration (ignored) - 0, 0 # Yaw, yaw_rate + 0, 0, 0, + vx, vy, vz, + 0, 0, 0, + 0, 0 ) 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...") + """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.time() + start = time.time() timeout = 30 + last_alt = 0 + stuck_count = 0 - while time.time() - start_time < timeout: + while time.time() - start < timeout: self.update_state() - if self.altitude >= altitude * 0.9: + 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 up thrust + # Ramp 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) + # Check if stuck + if thrust >= max_thrust - 0.05: + if abs(self.altitude - last_alt) < 0.01: + stuck_count += 1 + if stuck_count > 30: + print(f"\n[WARN] Not climbing at thrust={thrust:.2f}") + break + else: + stuck_count = 0 + last_alt = self.altitude - print(f"\r[CTRL] Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="") + 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 + 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=30): """Fly to position using velocity commands.""" @@ -377,7 +396,12 @@ class AutonomousController: else: vx, vy = 0, 0 - vz = -1.0 if dz > 0.5 else (0.5 if dz < -0.5 else -dz) + if dz > 0.5: + vz = -1.0 + elif dz < -0.5: + vz = 0.5 + else: + vz = -dz self.send_velocity_ned(vx, vy, vz) @@ -386,35 +410,34 @@ class AutonomousController: time.sleep(0.1) self.send_velocity_ned(0, 0, 0) - print(f"\n[CTRL] Timeout reaching waypoint") + print(f"\n[CTRL] Waypoint timeout") return False def land(self): - """Land the vehicle.""" + """Land the drone.""" print("[CTRL] Landing...") self.set_mode("LAND") - start_time = time.time() - while time.time() - start_time < 60: + 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 mission.""" - print(f"\n[MISSION] Hover at {altitude}m for {duration}s") + """Hover in place.""" + print(f"\n{'='*50}") + print(f" HOVER MISSION: {altitude}m for {duration}s") + print(f"{'='*50}\n") - self.setup_for_flight() + self.setup_gps_denied() + self.wait_for_ready() - if not self.wait_for_ekf(): - return False - - # Try GUIDED_NOGPS first, fallback to GUIDED if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') @@ -422,27 +445,31 @@ class AutonomousController: return False if not self.takeoff(altitude): + self.land() return False - print(f"[MISSION] Hovering for {duration} seconds...") + 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) # Maintain hover + 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="") time.sleep(0.1) + print() self.land() - print("[MISSION] Hover mission complete!") + print("\n[CTRL] Hover mission complete!") return True - def run_square_mission(self, altitude=5.0, side=5.0): + def run_square_mission(self, altitude=5.0, size=5.0): """Fly a square pattern.""" - print(f"\n[MISSION] Square pattern: {side}m sides at {altitude}m") + print(f"\n{'='*50}") + print(f" SQUARE MISSION: {size}m x {size}m at {altitude}m") + print(f"{'='*50}\n") - self.setup_for_flight() - - if not self.wait_for_ekf(): - return False + self.setup_gps_denied() + self.wait_for_ready() if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') @@ -451,6 +478,7 @@ class AutonomousController: return False if not self.takeoff(altitude): + self.land() return False self.update_state() @@ -458,29 +486,31 @@ class AutonomousController: start_y = self.position["y"] waypoints = [ - (start_x + side, start_y), - (start_x + side, start_y + side), - (start_x, start_y + side), + (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[MISSION] Waypoint {i+1}/4") + print(f"\n--- Waypoint {i+1}/4 ---") self.fly_to(x, y, altitude) time.sleep(1) self.land() - print("[MISSION] Square mission complete!") + 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[MISSION] Circle pattern: {radius}m radius at {altitude}m") + print(f"\n{'='*50}") + print(f" CIRCLE MISSION: radius={radius}m at {altitude}m") + print(f"{'='*50}\n") - self.setup_for_flight() - - if not self.wait_for_ekf(): - return False + self.setup_gps_denied() + self.wait_for_ready() if not self.set_mode('GUIDED_NOGPS'): self.set_mode('GUIDED') @@ -489,51 +519,58 @@ class AutonomousController: 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[MISSION] Point {i+1}/{points+1}") + print(f"\n--- Point {i+1}/{points+1} ---") self.fly_to(x, y, altitude) time.sleep(0.5) self.land() - print("[MISSION] Circle mission complete!") + print("\n[CTRL] Circle mission complete!") return True def main(): parser = argparse.ArgumentParser(description='Autonomous UAV Controller') - parser.add_argument('--connection', default='tcp:127.0.0.1:5760', + parser.add_argument('--connection', '-c', default='tcp:127.0.0.1:5760', help='MAVLink connection string') - parser.add_argument('--mission', choices=['hover', 'square', 'circle'], + parser.add_argument('--mission', '-m', choices=['hover', 'square', 'circle'], default='hover', help='Mission type') - parser.add_argument('--altitude', type=float, default=5.0, - help='Flight altitude in meters') - parser.add_argument('--size', type=float, default=5.0, - help='Mission size (side length or radius)') - parser.add_argument('--duration', type=float, default=30.0, - help='Hover duration in seconds') + 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) - print() + + controller = AutonomousController(args.connection) + + if not controller.connect(): + print("\n[ERROR] Could not connect to SITL") + sys.exit(1) try: - controller = AutonomousController(args.connection) - if args.mission == 'hover': controller.run_hover_mission(args.altitude, args.duration) elif args.mission == 'square': @@ -542,11 +579,13 @@ def main(): controller.run_circle_mission(args.altitude, args.size) except KeyboardInterrupt: - print("\n[CTRL] Interrupted by user") + print("\n\n[CTRL] Interrupted - Landing...") + controller.land() except Exception as e: - print(f"[ERROR] {e}") + print(f"\n[ERROR] {e}") import traceback traceback.print_exc() + controller.land() if __name__ == '__main__':