41 lines
918 B
Python
41 lines
918 B
Python
#!/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'])
|