Scripts Update 2

2026-02-09 06:12:38 +00:00
parent 347a5cc4f8
commit 1f4c405824
2 changed files with 78 additions and 50 deletions
--- 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
+
--- a/src/autonomous_controller.py
+++ b/src/autonomous_controller.py
@@ -93,31 +93,64 @@ 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."""
        print("[CTRL] Waiting for EKF initialization...")