Added Docker Files

src/drone_controller.py

@@ -390,9 +390,14 @@ class DroneController:
             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
+        # type_mask bits:
+        # bit 0: ignore body roll rate
+        # bit 1: ignore body pitch rate  
+        # bit 2: ignore body yaw rate
+        # bit 6: ignore thrust
+        # bit 7: attitude (quaternion) is valid
+        # We want: use quaternion (clear bit 7), use thrust (clear bit 6), ignore body rates (set bits 0-2)
+        type_mask = 0b00000111  # Use attitude + thrust, ignore body rates
         
         self.mav.mav.set_attitude_target_send(
             0,  # time_boot_ms
@@ -404,41 +409,79 @@ class DroneController:
             thrust          # Thrust 0-1
         )
     
-    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)...")
+    def _send_velocity_ned(self, vx, vy, vz):
+        """Send velocity command in NED frame."""
+        # type_mask bits (1=ignore, 0=use):
+        # bits 0-2: position x,y,z
+        # bits 3-5: velocity x,y,z  
+        # bits 6-8: acceleration x,y,z
+        # bit 9: force
+        # bit 10: yaw
+        # bit 11: yaw_rate
+        # Standard velocity-only mask: ignore pos, use vel, ignore accel, ignore yaw
+        type_mask = (
+            (1 << 0) | (1 << 1) | (1 << 2) |  # ignore position
+            # velocity bits 3,4,5 are 0 (use)
+            (1 << 6) | (1 << 7) | (1 << 8) |  # ignore acceleration
+            (1 << 10) | (1 << 11)              # ignore yaw, yaw_rate
+        )  # = 0b110111000111 = 0xDC7
         
         self.mav.mav.set_position_target_local_ned_send(
             0,
             self.mav.target_system,
             self.mav.target_component,
             mavutil.mavlink.MAV_FRAME_LOCAL_NED,
-            0b0000111111111000,  # Position only
-            x, y, z,  # Position (NED)
-            0, 0, 0,  # Velocity
-            0, 0, 0,  # Acceleration
-            0, 0  # Yaw, yaw_rate
+            type_mask,
+            0, 0, 0,      # Position (ignored)
+            vx, vy, vz,   # Velocity NED
+            0, 0, 0,      # Acceleration (ignored)
+            0, 0          # Yaw, yaw_rate
         )
     
-    def fly_to_and_wait(self, x, y, altitude, tolerance=0.5, timeout=30):
-        """Fly to position and wait until reached."""
-        z = -altitude  # NED
-        self.goto(x, y, z)
+    def fly_to_and_wait_absolute(self, target_x, target_y, altitude, tolerance=1.0, timeout=30):
+        """Fly to absolute position using velocity commands."""
+        
+        print(f"[INFO] Flying to ({target_x:.1f}, {target_y:.1f}, {altitude:.1f}m)")
         
         for i in range(int(timeout * 10)):
             self.update_state()
-            dx = x - self.position["x"]
-            dy = y - self.position["y"]
-            dist = math.sqrt(dx*dx + dy*dy)
             
-            if dist < tolerance:
-                print(f"[OK] Reached waypoint ({x:.1f}, {y:.1f})")
+            dx = target_x - self.position["x"]
+            dy = target_y - self.position["y"]
+            dz = altitude - self.altitude
+            
+            dist_horiz = math.sqrt(dx*dx + dy*dy)
+            
+            if dist_horiz < tolerance and abs(dz) < tolerance:
+                # Stop
+                self._send_velocity_ned(0, 0, 0)
+                print(f" [OK] Reached ({self.position['x']:.1f}, {self.position['y']:.1f})")
                 return True
             
+            # Calculate velocity towards target (in NED frame)
+            speed = min(2.0, max(0.5, dist_horiz * 0.5))  # 0.5-2.0 m/s
+            
+            if dist_horiz > 0.1:
+                vx = (dx / dist_horiz) * speed
+                vy = (dy / dist_horiz) * speed
+            else:
+                vx, vy = 0, 0
+            
+            # Vertical velocity
+            if dz > 0.5:
+                vz = -1.0  # Climb (negative = up in NED)
+            elif dz < -0.5:
+                vz = 0.5   # Descend
+            else:
+                vz = -dz   # Small adjustment
+            
+            self._send_velocity_ned(vx, vy, vz)
+            
             if i % 10 == 0:
-                print(f"\r    Distance: {dist:.1f}m", end="")
+                print(f"\r    Pos: ({self.position['x']:.1f}, {self.position['y']:.1f}) -> ({target_x:.1f}, {target_y:.1f}) Dist: {dist_horiz:.1f}m Alt: {self.altitude:.1f}m", end="")
             time.sleep(0.1)
         
+        self._send_velocity_ned(0, 0, 0)
         print(f"\n[WARN] Timeout reaching waypoint")
         return False
     
@@ -448,33 +491,46 @@ class DroneController:
         return self.set_mode("LAND")
     
     def fly_square(self, size=5, altitude=5):
-        """Fly a square pattern."""
+        """Fly a square pattern starting from current position."""
+        # Store initial position
+        self.update_state()
+        start_x = self.position["x"]
+        start_y = self.position["y"]
+        
+        # Waypoints are absolute positions based on start
         waypoints = [
-            (size, 0),
-            (size, size),
-            (0, size),
-            (0, 0),
+            (start_x + size, start_y),         # Forward
+            (start_x + size, start_y + size),  # Right
+            (start_x, start_y + size),         # Back
+            (start_x, start_y),                # Left (return)
         ]
         
         print(f"\n[INFO] Flying square pattern ({size}m x {size}m)")
+        print(f"[INFO] Start position: ({start_x:.1f}, {start_y:.1f})")
         
         for i, (x, y) in enumerate(waypoints):
-            print(f"\n--- Waypoint {i+1}/4 ---")
-            self.fly_to_and_wait(x, y, altitude)
+            print(f"\n--- Waypoint {i+1}/4 ({x:.1f}, {y:.1f}) ---")
+            self.fly_to_and_wait_absolute(x, y, altitude)
             time.sleep(1)
         
         print("\n[OK] Square pattern complete!")
     
     def fly_circle(self, radius=5, altitude=5, points=8):
-        """Fly a circular pattern."""
+        """Fly a circular pattern starting from current position."""
+        # Store initial position (center of circle)
+        self.update_state()
+        center_x = self.position["x"]
+        center_y = self.position["y"]
+        
         print(f"\n[INFO] Flying circle pattern (radius={radius}m)")
+        print(f"[INFO] Center: ({center_x:.1f}, {center_y:.1f})")
         
         for i in range(points + 1):
             angle = 2 * math.pi * i / points
-            x = radius * math.cos(angle)
-            y = radius * math.sin(angle)
-            print(f"\n--- Point {i+1}/{points+1} ---")
-            self.fly_to_and_wait(x, y, altitude)
+            x = center_x + radius * math.cos(angle)
+            y = center_y + radius * math.sin(angle)
+            print(f"\n--- Point {i+1}/{points+1} ({x:.1f}, {y:.1f}) ---")
+            self.fly_to_and_wait_absolute(x, y, altitude)
             time.sleep(0.5)
         
         print("\n[OK] Circle pattern complete!")