diff --git a/src/drone_controller.py b/src/drone_controller.py
index 4c34c2d..08f2d73 100644
--- a/src/drone_controller.py
+++ b/src/drone_controller.py
@@ -46,24 +46,47 @@ class DroneController:
         
         try:
             self.mav = mavutil.mavlink_connection(self.connection_string)
-            msg = self.mav.wait_heartbeat(timeout=timeout)
-            print(f"[OK] Connected to system {self.mav.target_system}")
+            
+            # Wait for heartbeat with retries
+            print("[INFO] 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"[WARN] Heartbeat attempt {attempt + 1}/3 - retrying...")
+            
+            if not msg or self.mav.target_system == 0:
+                print("[ERROR] Failed to receive valid heartbeat")
+                print("[TIP] Make sure SITL is running:")
+                print("      sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console")
+                return False
+            
+            print(f"[OK] Connected to system {self.mav.target_system}, component {self.mav.target_component}")
             
             # Detect vehicle type from heartbeat
-            if msg:
-                mav_type = msg.type
-                if mav_type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
-                    self.vehicle_type = "copter"
-                    print(f"[INFO] Vehicle: Quadrotor (ArduCopter)")
-                elif mav_type == mavutil.mavlink.MAV_TYPE_HELICOPTER:
-                    self.vehicle_type = "copter"
-                    print(f"[INFO] Vehicle: Helicopter")
-                elif mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
-                    self.vehicle_type = "plane"
-                    print(f"[INFO] Vehicle: Fixed Wing (ArduPlane)")
-                else:
-                    self.vehicle_type = "copter"  # Default to copter
-                    print(f"[INFO] Vehicle type: {mav_type}")
+            mav_type = msg.type
+            if mav_type == mavutil.mavlink.MAV_TYPE_QUADROTOR:
+                self.vehicle_type = "copter"
+                print(f"[INFO] Vehicle: Quadrotor (ArduCopter)")
+            elif mav_type == mavutil.mavlink.MAV_TYPE_HELICOPTER:
+                self.vehicle_type = "copter"
+                print(f"[INFO] Vehicle: Helicopter")
+            elif mav_type == mavutil.mavlink.MAV_TYPE_FIXED_WING:
+                self.vehicle_type = "plane"
+                print(f"[INFO] Vehicle: Fixed Wing (ArduPlane)")
+            else:
+                self.vehicle_type = "copter"  # Default to copter
+                print(f"[INFO] Vehicle type ID: {mav_type}")
+            
+            # 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,
+                4,  # 4 Hz
+                1   # Start
+            )
             
             return True
         except Exception as e: