Initial Commit

tests/test_uav_control.py

@@ -0,0 +1,40 @@
+#!/usr/bin/env python3
+"""Test UAV control logic."""
+
+import pytest
+import numpy as np
+
+
+def test_waypoint_distance():
+    current = np.array([0, 0, 5])
+    target = np.array([10, 0, 5])
+    distance = np.linalg.norm(target - current)
+    assert np.isclose(distance, 10.0)
+
+
+def test_velocity_command():
+    current = np.array([0, 0, 5])
+    target = np.array([10, 0, 5])
+    
+    direction = target - current
+    distance = np.linalg.norm(direction)
+    direction = direction / distance
+    
+    max_vel = 2.0
+    speed = min(max_vel, distance * 0.5)
+    velocity = direction * speed
+    
+    assert np.isclose(velocity[0], max_vel)
+    assert np.isclose(velocity[1], 0.0)
+
+
+def test_position_hold():
+    current = np.array([5.0, 3.0, 10.0])
+    target = current.copy()
+    
+    error = np.linalg.norm(target - current)
+    assert np.isclose(error, 0.0)
+
+
+if __name__ == '__main__':
+    pytest.main([__file__, '-v'])