drone controller update 4

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
+        # Thrust values: 0=min, 1=max. Hover is typically 0.5-0.7 depending on weight
-        hover_thrust = 0.5
+        # For Gazebo iris model, we may need higher thrust
-        climb_thrust = 0.65  # Slightly above hover to climb
+        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
+            # Ramp up thrust if not climbing
-            if abs(self.altitude - last_alt) < 0.01:
+            if self.altitude < 0.5 and thrust < max_climb_thrust:
-                stuck_count += 1
+                thrust = min(thrust + 0.01, max_climb_thrust)  # Gradually increase
-                if stuck_count > 50:  # 5 seconds stuck
+            elif self.altitude > 0.5:
-                    print(f"\n[WARN] Drone not climbing - check motor output")
+                # We're flying! Maintain good climb thrust
-                    break
+                thrust = 0.75
-            else:
+            
-                stuck_count = 0
+            # 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