From 389eed93415269d3e1c0834906f50e573e0df090 Mon Sep 17 00:00:00 2001
From: default <adithcool.5@gmail.com>
Date: Fri, 9 Jan 2026 20:29:31 +0000
Subject: [PATCH] drone controller update 4

---
 src/drone_controller.py | 37 ++++++++++++++++++++++++-------------
 1 file changed, 24 insertions(+), 13 deletions(-)

diff --git a/src/drone_controller.py b/src/drone_controller.py
index 1ea158b..1c59ac4 100644
--- a/src/drone_controller.py
+++ b/src/drone_controller.py
@@ -311,14 +311,16 @@ class DroneController:
         print(f"[INFO] Taking off to {altitude}m...")
         
         # In GUIDED_NOGPS, we need to use attitude+thrust commands
-        # Hover thrust is approximately 0.5-0.6 for a balanced quad
-        hover_thrust = 0.5
-        climb_thrust = 0.65  # Slightly above hover to climb
+        # Thrust values: 0=min, 1=max. Hover is typically 0.5-0.7 depending on weight
+        # For Gazebo iris model, we may need higher thrust
+        hover_thrust = 0.6
+        max_climb_thrust = 0.85  # High thrust for takeoff
         
         start_time = time.time()
         timeout = 30  # seconds
         last_alt = 0
         stuck_count = 0
+        thrust = 0.5  # Start low and ramp up
         
         while time.time() - start_time < timeout:
             self.update_state()
@@ -331,20 +333,29 @@ class DroneController:
                 time.sleep(0.5)  # Stabilize
                 return True
             
-            # Check if making progress
-            if abs(self.altitude - last_alt) < 0.01:
-                stuck_count += 1
-                if stuck_count > 50:  # 5 seconds stuck
-                    print(f"\n[WARN] Drone not climbing - check motor output")
-                    break
-            else:
-                stuck_count = 0
+            # Ramp up thrust if not climbing
+            if self.altitude < 0.5 and thrust < max_climb_thrust:
+                thrust = min(thrust + 0.01, max_climb_thrust)  # Gradually increase
+            elif self.altitude > 0.5:
+                # We're flying! Maintain good climb thrust
+                thrust = 0.75
+            
+            # Check if making progress (after ramping)
+            if thrust >= max_climb_thrust - 0.05:
+                if abs(self.altitude - last_alt) < 0.01:
+                    stuck_count += 1
+                    if stuck_count > 30:  # 3 seconds stuck at max thrust
+                        print(f"\n[WARN] Drone not climbing at thrust={thrust:.2f}")
+                        print("[TIP] Check Gazebo physics or motor configuration")
+                        break
+                else:
+                    stuck_count = 0
             last_alt = self.altitude
             
             # Send climb thrust command (level attitude, thrust up)
-            self._send_attitude_thrust(0, 0, 0, climb_thrust)
+            self._send_attitude_thrust(0, 0, 0, thrust)
             
-            print(f"\r    Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={climb_thrust:.2f})", end="")
+            print(f"\r    Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={thrust:.2f})", end="")
             time.sleep(0.1)
         
         # Stop - reduce to hover thrust