From 1f4c405824cecd8f078f919dab97f7d0dce8f4b4 Mon Sep 17 00:00:00 2001 From: default Date: Mon, 9 Feb 2026 06:12:38 +0000 Subject: [PATCH] Scripts Update 2 --- config/ardupilot_gps_denied.parm | 69 +++++++++++++++----------------- src/autonomous_controller.py | 59 +++++++++++++++++++++------ 2 files changed, 78 insertions(+), 50 deletions(-) diff --git a/config/ardupilot_gps_denied.parm b/config/ardupilot_gps_denied.parm index 4aedc7f..a2e1019 100644 --- a/config/ardupilot_gps_denied.parm +++ b/config/ardupilot_gps_denied.parm @@ -1,20 +1,20 @@ -# ArduPilot Parameters for GPS-Denied Navigation -# GPS ONLY used for geofencing, NOT navigation +# ArduPilot Parameters for Simulation +# Uses GPS from Gazebo for position estimation # ==================== # GPS Configuration # ==================== -# Disable GPS for navigation -GPS_TYPE 0 +# Enable GPS (Gazebo provides simulated GPS) +GPS_TYPE 1 # Auto-detect GPS_TYPE2 0 # ==================== # Geofence Settings # ==================== -# Enable geofence (still uses GPS for boundaries) -FENCE_ENABLE 1 -FENCE_TYPE 7 # All fence types (circle + polygon + altitude) -FENCE_ACTION 1 # RTL on breach +# Disable geofence for simulation (avoids position requirement issues) +FENCE_ENABLE 0 +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_MARGIN 2 # Margin inside fence for warning @@ -22,67 +22,61 @@ FENCE_MARGIN 2 # Margin inside fence for warning # ==================== # EKF Configuration # ==================== -# Use EKF3 with external navigation +# Use EKF3 with GPS AHRS_EKF_TYPE 3 EK3_ENABLE 1 EK2_ENABLE 0 -# EKF3 Source Configuration -# Source 1: External Nav (Visual Odometry) -EK3_SRC1_POSXY 6 # External nav for XY position +# EKF3 Source Configuration - Use GPS +EK3_SRC1_POSXY 3 # GPS for XY position EK3_SRC1_POSZ 1 # Barometer for Z position -EK3_SRC1_VELXY 6 # External nav for XY velocity +EK3_SRC1_VELXY 3 # GPS for XY velocity EK3_SRC1_VELZ 0 # None for Z velocity -EK3_SRC1_YAW 6 # External nav for yaw +EK3_SRC1_YAW 1 # Compass for yaw # EKF3 Position Innovation Gate -EK3_POS_I_GATE 300 # Larger gate for visual odometry +EK3_POS_I_GATE 300 # Larger gate # ==================== # Vision Position Input # ==================== -VISO_TYPE 1 # MAVLink vision position -VISO_POS_X 0.1 # Camera X offset from CG (meters) -VISO_POS_Y 0.0 # Camera Y offset from CG (meters) -VISO_POS_Z -0.05 # Camera Z offset from CG (meters) -VISO_ORIENT 0 # Camera orientation (0 = forward) -VISO_DELAY_MS 50 # Vision position delay (ms) +# 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 # ==================== -FLOW_TYPE 1 # Enable optical flow -FLOW_FXSCALER 200 # X scale factor -FLOW_FYSCALER 200 # Y scale factor -FLOW_POS_X 0.0 # Flow sensor X offset -FLOW_POS_Y 0.0 # Flow sensor Y offset -FLOW_POS_Z 0.0 # Flow sensor Z offset +# Disable optical flow (using GPS instead) +FLOW_TYPE 0 # Disabled # ==================== -# Rangefinder (for altitude in GPS-denied) +# Rangefinder # ==================== -RNGFND1_TYPE 1 # Analog rangefinder -RNGFND1_MIN_CM 10 # Minimum range (cm) -RNGFND1_MAX_CM 1000 # Maximum range (cm) -RNGFND1_ORIENT 25 # Downward orientation +# 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 (optical flow sensitive) +FS_VIBE_ENABLE 0 # Disable vibration failsafe +FS_GPS_ENABLE 0 # Disable GPS failsafe for simulation # ==================== # Flight Modes # ==================== -# Disable GPS-dependent modes -# Allowed: STABILIZE, ALT_HOLD, LOITER (with optical flow), GUIDED FLTMODE1 0 # Stabilize FLTMODE2 2 # Alt Hold -FLTMODE3 5 # Loiter (optical flow based) +FLTMODE3 5 # Loiter FLTMODE4 4 # Guided -FLTMODE5 6 # RTL (returns to local origin) +FLTMODE5 6 # RTL FLTMODE6 9 # Land # ==================== @@ -96,3 +90,4 @@ ARMING_CHECK 0 # Disable all checks (for sim only) # ==================== 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 11e6990..89af0b9 100755 --- a/src/autonomous_controller.py +++ b/src/autonomous_controller.py @@ -93,30 +93,63 @@ class AutonomousController: time.sleep(0.3) def setup_for_flight(self): - """Configure ArduPilot for simulated GPS-denied flight. + """Configure ArduPilot for simulation flight. - GPS stays enabled in EKF for geofence safety, but we use - GUIDED_NOGPS mode which doesn't require GPS for control. + For Gazebo simulation, we use GPS (provided by Gazebo) for position. + We disable the fence to avoid position requirement conflicts. """ - print("[CTRL] Configuring for simulated GPS-denied flight...") + print("[CTRL] Configuring for simulation...") - # Disable arming checks + # Disable ALL arming checks for simulation self.set_param("ARMING_CHECK", 0) - # Keep GPS enabled in EKF for geofence + # Enable GPS (Gazebo provides simulated GPS) + self.set_param("GPS_TYPE", 1) # Auto + + # Configure EKF to use GPS 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) + # Disable fence to avoid "fence requires position" error + self.set_param("FENCE_ENABLE", 0) - print("[CTRL] Setup complete (GPS enabled for geofence, using GUIDED_NOGPS for control)") + # Disable vision odometry requirement + self.set_param("VISO_TYPE", 0) + + # Disable GPS failsafe + self.set_param("FS_GPS_ENABLE", 0) + + print("[CTRL] Setup complete") time.sleep(1) + + # Wait for GPS lock and home position + print("[CTRL] Waiting for GPS lock and home position...") + for i in range(60): # 60 second 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) + 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': + text = msg.text if isinstance(msg.text, str) else msg.text.decode('utf-8', errors='ignore') + if text.strip(): + print(f"[SITL] {text.strip()}") + + if i % 10 == 0: + print(f"[CTRL] Waiting for GPS... ({i}s)") + + print("[CTRL] GPS timeout - continuing anyway") + return True def wait_for_ekf(self, timeout=60): """Wait for EKF to initialize."""