SirBlob/simulation
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Scripts Update 2

Browse Source
This commit is contained in:
default
2026-02-09 06:12:38 +00:00
parent 347a5cc4f8
commit 1f4c405824
2 changed files with 78 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

69
config/ardupilot_gps_denied.parm
View File

@@ -1,20 +1,20 @@
# ArduPilot Parameters for GPS-Denied Navigation # ArduPilot Parameters for Simulation
# GPS ONLY used for geofencing, NOT navigation # Uses GPS from Gazebo for position estimation
# ==================== # ====================
# GPS Configuration # GPS Configuration
# ==================== # ====================
# Disable GPS for navigation # Enable GPS (Gazebo provides simulated GPS)
GPS_TYPE 0 GPS_TYPE 1 # Auto-detect
GPS_TYPE2 0 GPS_TYPE2 0
# ==================== # ====================
# Geofence Settings # Geofence Settings
# ==================== # ====================
# Enable geofence (still uses GPS for boundaries) # Disable geofence for simulation (avoids position requirement issues)
FENCE_ENABLE 1 FENCE_ENABLE 0
FENCE_TYPE 7 # All fence types (circle + polygon + altitude) FENCE_TYPE 3 # Alt + Circle
FENCE_ACTION 1 # RTL on breach FENCE_ACTION 2 # Land on breach
FENCE_ALT_MAX 50 # Maximum altitude (meters) FENCE_ALT_MAX 50 # Maximum altitude (meters)
FENCE_RADIUS 100 # Circular fence radius (meters) FENCE_RADIUS 100 # Circular fence radius (meters)
FENCE_MARGIN 2 # Margin inside fence for warning FENCE_MARGIN 2 # Margin inside fence for warning
@@ -22,67 +22,61 @@ FENCE_MARGIN 2 # Margin inside fence for warning
# ==================== # ====================
# EKF Configuration # EKF Configuration
# ==================== # ====================
# Use EKF3 with external navigation # Use EKF3 with GPS
AHRS_EKF_TYPE 3 AHRS_EKF_TYPE 3
EK3_ENABLE 1 EK3_ENABLE 1
EK2_ENABLE 0 EK2_ENABLE 0
# EKF3 Source Configuration # EKF3 Source Configuration - Use GPS
# Source 1: External Nav (Visual Odometry) EK3_SRC1_POSXY 3 # GPS for XY position
EK3_SRC1_POSXY 6 # External nav for XY position
EK3_SRC1_POSZ 1 # Barometer for Z 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_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 # EKF3 Position Innovation Gate
EK3_POS_I_GATE 300 # Larger gate for visual odometry EK3_POS_I_GATE 300 # Larger gate
# ==================== # ====================
# Vision Position Input # Vision Position Input
# ==================== # ====================
VISO_TYPE 1 # MAVLink vision position # Disable vision odometry (not using for simulation)
VISO_POS_X 0.1 # Camera X offset from CG (meters) VISO_TYPE 0 # Disabled
VISO_POS_Y 0.0 # Camera Y offset from CG (meters) VISO_POS_X 0.0
VISO_POS_Z -0.05 # Camera Z offset from CG (meters) VISO_POS_Y 0.0
VISO_ORIENT 0 # Camera orientation (0 = forward) VISO_POS_Z 0.0
VISO_DELAY_MS 50 # Vision position delay (ms) VISO_ORIENT 0
VISO_DELAY_MS 50
# ==================== # ====================
# Optical Flow # Optical Flow
# ==================== # ====================
FLOW_TYPE 1 # Enable optical flow # Disable optical flow (using GPS instead)
FLOW_FXSCALER 200 # X scale factor FLOW_TYPE 0 # Disabled
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
# ==================== # ====================
# Rangefinder (for altitude in GPS-denied) # Rangefinder
# ==================== # ====================
RNGFND1_TYPE 1 # Analog rangefinder # Disable rangefinder (using barometer + GPS)
RNGFND1_MIN_CM 10 # Minimum range (cm) RNGFND1_TYPE 0 # Disabled
RNGFND1_MAX_CM 1000 # Maximum range (cm)
RNGFND1_ORIENT 25 # Downward orientation
# ==================== # ====================
# Failsafe Settings # Failsafe Settings
# ==================== # ====================
FS_EKF_ACTION 1 # Land on EKF failsafe FS_EKF_ACTION 1 # Land on EKF failsafe
FS_EKF_THRESH 0.8 # EKF failsafe threshold 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 # Flight Modes
# ==================== # ====================
# Disable GPS-dependent modes
# Allowed: STABILIZE, ALT_HOLD, LOITER (with optical flow), GUIDED
FLTMODE1 0 # Stabilize FLTMODE1 0 # Stabilize
FLTMODE2 2 # Alt Hold FLTMODE2 2 # Alt Hold
FLTMODE3 5 # Loiter (optical flow based) FLTMODE3 5 # Loiter
FLTMODE4 4 # Guided FLTMODE4 4 # Guided
FLTMODE5 6 # RTL (returns to local origin) FLTMODE5 6 # RTL
FLTMODE6 9 # Land FLTMODE6 9 # Land
# ==================== # ====================
@@ -96,3 +90,4 @@ ARMING_CHECK 0 # Disable all checks (for sim only)
# ==================== # ====================
LOG_BITMASK 176126 # Log all relevant data LOG_BITMASK 176126 # Log all relevant data
LOG_DISARMED 0 # Don't log when disarmed LOG_DISARMED 0 # Don't log when disarmed

59
src/autonomous_controller.py
View File

@@ -93,30 +93,63 @@ class AutonomousController:
time.sleep(0.3) time.sleep(0.3)
def setup_for_flight(self): 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 For Gazebo simulation, we use GPS (provided by Gazebo) for position.
GUIDED_NOGPS mode which doesn't require GPS for control. 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) 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_POSXY", 3) # GPS
self.set_param("EK3_SRC1_VELXY", 3) # GPS self.set_param("EK3_SRC1_VELXY", 3) # GPS
self.set_param("EK3_SRC1_POSZ", 1) # Baro self.set_param("EK3_SRC1_POSZ", 1) # Baro
# Setup geofence # Disable fence to avoid "fence requires position" error
self.set_param("FENCE_ENABLE", 1) self.set_param("FENCE_ENABLE", 0)
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)") # 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) 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): def wait_for_ekf(self, timeout=60):
"""Wait for EKF to initialize.""" """Wait for EKF to initialize."""