diff --git a/src/autonomous_controller.py b/src/autonomous_controller.py
index ca2c0df..8fdb48b 100755
--- a/src/autonomous_controller.py
+++ b/src/autonomous_controller.py
@@ -128,7 +128,7 @@ class AutonomousController:
         """Configure for simulated GPS-denied operation.
         
         GPS is enabled in EKF for:
-        1. Geofence safety
+        1. Geofence safety (enabled after GPS lock)
         2. LOCAL_POSITION_NED telemetry
         
         But we use GUIDED_NOGPS mode and send velocity/attitude commands,
@@ -147,48 +147,68 @@ class AutonomousController:
         # Disable GPS failsafe (we're simulating GPS-denied)
         self.set_param("FS_GPS_ENABLE", 0)
         
-        # 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)
+        # DISABLE fence initially - will enable after GPS lock
+        self.set_param("FENCE_ENABLE", 0)
         
-        print("[CTRL] Setup complete (GPS for fence, GUIDED_NOGPS for control)")
+        print("[CTRL] Setup complete (fence will enable after GPS lock)")
         time.sleep(1)
     
-    def wait_for_ready(self, timeout=60):
-        """Wait for EKF and GPS to be ready."""
-        print("[CTRL] Waiting for system ready...")
+    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()
             
-            msg = self.mav.recv_match(type=['STATUSTEXT', 'GPS_RAW_INT'], blocking=True, timeout=1)
+            # 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}")
-                    if 'EKF3' in text and 'active' in text.lower():
-                        print("[CTRL] EKF ready!")
-                        time.sleep(2)
-                        return True
+                    # 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:
-                        print(f"[CTRL] GPS fix: {msg.fix_type} (satellites: {msg.satellites_visible})")
-                        time.sleep(1)
-                        return True
+                    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!")
             
-            if int(time.time() - start) % 10 == 0 and int(time.time() - start) > 0:
-                print(f"[CTRL] Waiting... ({int(time.time() - start)}s)")
+            # Check if ready
+            if gps_ok and home_ok:
+                print("[CTRL] GPS and home ready!")
+                
+                # NOW enable geofence since we have position
+                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)
+                time.sleep(1)
+                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")
+        print("[CTRL] Ready timeout - continuing anyway (fence disabled)")
         return True
     
     def set_mode(self, mode_name):