drone controller update 3

src/drone_controller.py

@@ -307,54 +307,90 @@ class DroneController:
         return False
     
     def takeoff(self, altitude=5.0):
-        """Takeoff to altitude using velocity commands (works in GUIDED_NOGPS)."""
+        """Takeoff to altitude using thrust commands (works in GUIDED_NOGPS)."""
         print(f"[INFO] Taking off to {altitude}m...")
         
-        # In GUIDED_NOGPS, we can't use NAV_TAKEOFF (requires position estimate)
-        # Instead, send climb velocity commands
-        climb_rate = 1.0  # m/s upward
+        # 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
         
         start_time = time.time()
         timeout = 30  # seconds
+        last_alt = 0
+        stuck_count = 0
         
         while time.time() - start_time < timeout:
             self.update_state()
             
-            if self.altitude >= altitude * 0.95:
-                # Stop climbing, hover
-                self._send_velocity(0, 0, 0)
+            # Check if reached target altitude
+            if self.altitude >= altitude * 0.90:
+                # Reduce thrust to hover
+                self._send_attitude_thrust(0, 0, 0, hover_thrust)
                 print(f"\n[OK] Reached {self.altitude:.1f}m")
+                time.sleep(0.5)  # Stabilize
                 return True
             
-            # Send climb command (negative vz = up in NED)
-            self._send_velocity(0, 0, -climb_rate)
+            # 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
+            last_alt = self.altitude
             
-            print(f"\r    Climbing... {self.altitude:.1f}m / {altitude:.1f}m", end="")
+            # Send climb thrust command (level attitude, thrust up)
+            self._send_attitude_thrust(0, 0, 0, climb_thrust)
+            
+            print(f"\r    Climbing... {self.altitude:.1f}m / {altitude:.1f}m (thrust={climb_thrust:.2f})", end="")
             time.sleep(0.1)
         
-        # Stop any motion
-        self._send_velocity(0, 0, 0)
+        # Stop - reduce to hover thrust
+        self._send_attitude_thrust(0, 0, 0, hover_thrust)
         print(f"\n[WARN] Takeoff timeout at {self.altitude:.1f}m")
         return False
     
-    def _send_velocity(self, vx, vy, vz, yaw_rate=0):
-        """Send velocity command in body frame."""
-        # Use SET_POSITION_TARGET_LOCAL_NED with velocity only
-        # type_mask: ignore position, acceleration, yaw (use velocity only)
-        type_mask = (
-            0b0000_0001_11_000_111  # Ignore pos (0-2), accel (6-8), yaw (10), use vel (3-5) and yaw_rate (11)
-        )
+    def _send_attitude_thrust(self, roll, pitch, yaw, thrust):
+        """Send attitude and thrust command (for GUIDED_NOGPS mode).
         
-        self.mav.mav.set_position_target_local_ned_send(
-            0,  # time_boot_ms (ignored)
+        Args:
+            roll: Roll angle in radians
+            pitch: Pitch angle in radians  
+            yaw: Yaw angle in radians
+            thrust: Thrust 0.0-1.0 (0.5 = hover, 0.6+ = climb)
+        """
+        import math
+        
+        # Convert euler angles to quaternion
+        # Using simple conversion for small angles
+        cy = math.cos(yaw * 0.5)
+        sy = math.sin(yaw * 0.5)
+        cp = math.cos(pitch * 0.5)
+        sp = math.sin(pitch * 0.5)
+        cr = math.cos(roll * 0.5)
+        sr = math.sin(roll * 0.5)
+        
+        q = [
+            cr * cp * cy + sr * sp * sy,  # w
+            sr * cp * cy - cr * sp * sy,  # x
+            cr * sp * cy + sr * cp * sy,  # y
+            cr * cp * sy - sr * sp * cy   # z
+        ]
+        
+        # type_mask: bit 7 = ignore body roll/pitch rate
+        # We provide quaternion and thrust
+        type_mask = 0b00000111  # Ignore body rates, use quaternion + thrust
+        
+        self.mav.mav.set_attitude_target_send(
+            0,  # time_boot_ms
             self.mav.target_system,
             self.mav.target_component,
-            mavutil.mavlink.MAV_FRAME_BODY_NED,  # Body frame
             type_mask,
-            0, 0, 0,        # Position (ignored)
-            vx, vy, vz,     # Velocity
-            0, 0, 0,        # Acceleration (ignored)
-            0, yaw_rate     # Yaw, yaw_rate
+            q,              # Quaternion [w, x, y, z]
+            0, 0, 0,        # Body roll/pitch/yaw rates (ignored)
+            thrust          # Thrust 0-1
         )
     
     def goto(self, x, y, z):