From 433e97beb8cf9c1bfad005ac5909592355412dc7 Mon Sep 17 00:00:00 2001
From: default <adithcool.5@gmail.com>
Date: Fri, 9 Jan 2026 20:21:36 +0000
Subject: [PATCH] drone controller velocity update

---
 src/drone_controller.py | 52 +++++++++++++++++++++++++++++++----------
 1 file changed, 40 insertions(+), 12 deletions(-)

diff --git a/src/drone_controller.py b/src/drone_controller.py
index 08f2d73..6f6d674 100644
--- a/src/drone_controller.py
+++ b/src/drone_controller.py
@@ -307,28 +307,56 @@ class DroneController:
         return False
     
     def takeoff(self, altitude=5.0):
-        """Takeoff to altitude."""
+        """Takeoff to altitude using velocity commands (works in GUIDED_NOGPS)."""
         print(f"[INFO] Taking off to {altitude}m...")
         
-        self.mav.mav.command_long_send(
-            self.mav.target_system,
-            self.mav.target_component,
-            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
-            0, 0, 0, 0, 0, 0, 0, altitude
-        )
+        # In GUIDED_NOGPS, we can't use NAV_TAKEOFF (requires position estimate)
+        # Instead, send climb velocity commands
+        climb_rate = 1.0  # m/s upward
         
-        # Wait for altitude
-        for i in range(100):  # 10 seconds max
+        start_time = time.time()
+        timeout = 30  # seconds
+        
+        while time.time() - start_time < timeout:
             self.update_state()
-            if self.altitude > altitude * 0.9:
-                print(f"[OK] Reached {self.altitude:.1f}m")
+            
+            if self.altitude >= altitude * 0.95:
+                # Stop climbing, hover
+                self._send_velocity(0, 0, 0)
+                print(f"\n[OK] Reached {self.altitude:.1f}m")
                 return True
-            print(f"\r    Climbing... {self.altitude:.1f}m", end="")
+            
+            # Send climb command (negative vz = up in NED)
+            self._send_velocity(0, 0, -climb_rate)
+            
+            print(f"\r    Climbing... {self.altitude:.1f}m / {altitude:.1f}m", end="")
             time.sleep(0.1)
         
+        # Stop any motion
+        self._send_velocity(0, 0, 0)
         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)
+        )
+        
+        self.mav.mav.set_position_target_local_ned_send(
+            0,  # time_boot_ms (ignored)
+            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
+        )
+    
     def goto(self, x, y, z):
         """Go to position (NED frame, z is down so negative = up)."""
         print(f"[INFO] Going to ({x:.1f}, {y:.1f}, alt={-z:.1f}m)...")