44 lines
1.6 KiB
Python
44 lines
1.6 KiB
Python
"""frame_transform_test.py —— rotate_body_to_ned 单元测试。
|
|
|
|
移植自 reference-c++/visual_track_control/tests/frame_transform_test.cpp。
|
|
覆盖（物理追踪思路 §2.3，A4）：零航向恒等、朝东时机体前向 → 东向、
|
|
垂直分量不变、模长守恒。
|
|
"""
|
|
|
|
import math
|
|
|
|
import pytest
|
|
|
|
from visual_track_control.frame_transform import rotate_body_to_ned
|
|
|
|
|
|
def test_zero_yaw_is_identity():
|
|
north, east, down = rotate_body_to_ned(1.0, 0.5, -2.0, 0.0)
|
|
assert north == pytest.approx(1.0, abs=1e-12) # 北 = 前
|
|
assert east == pytest.approx(0.5, abs=1e-12) # 东 = 右
|
|
assert down == pytest.approx(-2.0, abs=1e-12)
|
|
|
|
|
|
def test_yaw_east_90_maps_forward_to_east():
|
|
north, east, _ = rotate_body_to_ned(2.0, 0.0, 0.0, math.pi / 2.0) # 机头朝东
|
|
assert north == pytest.approx(0.0, abs=1e-9) # 北向 ≈ 0
|
|
assert east == pytest.approx(2.0, abs=1e-9) # 东向 = 前向速度
|
|
|
|
|
|
def test_right_is_east_when_heading_north():
|
|
north, east, _ = rotate_body_to_ned(0.0, 1.0, 0.0, 0.0)
|
|
assert north == pytest.approx(0.0, abs=1e-12)
|
|
assert east == pytest.approx(1.0, abs=1e-12)
|
|
|
|
|
|
def test_down_component_unchanged():
|
|
_, _, down = rotate_body_to_ned(1.0, 1.0, 3.5, math.pi / 3.0)
|
|
assert down == 3.5
|
|
|
|
|
|
def test_preserves_horizontal_norm():
|
|
# 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
|
|
north, east, _ = rotate_body_to_ned(3.0, -4.0, 1.0, 0.7)
|
|
assert math.hypot(north, east) == pytest.approx(math.hypot(3.0, -4.0),
|
|
abs=1e-9)
|