songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

双目测距方案重构与其他小修改

Browse Source
This commit is contained in:
keita
2026-09-10 16:16:05 +08:00
parent 3325465359
commit 538ae4c6c7
39 changed files with 2285 additions and 1444 deletions
Download Patch File Download Diff File Expand all files Collapse all files
utils/tests/visual_track_control/tracking_control_law_test.py
+85 -34
View File
@@ -3,8 +3,9 @@
移植自 reference-c++/visual_track_control/tests/tracking_control_law_test.cpp。
覆盖（物理追踪思路 §2.3/§4/§5/§6/§7/§9）：超龄降级（姿态缺失/过期、视觉
超龄、降级优先级）、判丢零速刹车、Coast 清积分与标志、速率式/位置式航向
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道三种
无距离降级动作、首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
通道（含绝对航向合成与回绕，A2）、垂直统一速度支路（A1）、距离通道无距离
降级动作（含 EXIT 门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率
限幅（S6）、机体系→NED 输出旋转（A4）。
"""
import math
@@ -26,7 +27,7 @@ def make_config() -> TrackingConfig:
cfg = TrackingConfig()
cfg.control.frequency_hz = 20.0
cfg.control.visual_stale_ms = 200
cfg.control.radar_stale_ms = 500
cfg.control.distance_stale_ms = 500
cfg.control.attitude_stale_ms = 300
cfg.camera.image_width = 1000
cfg.camera.image_height = 1000
@@ -55,7 +56,7 @@ def make_config() -> TrackingConfig:
def make_snapshot(now_ms: int) -> ControlSnapshot:
"""新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜。"""
"""新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜。"""
return ControlSnapshot(
track=TrackState(tracked=True, is_predicted=False,
center=(500.0, 500.0)),
@@ -213,9 +214,9 @@ def test_vertical_velocity_clamped():
def test_distance_approach_when_far():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0 # Δd = +20 → kp 输出 10 → 限幅 3
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
@@ -223,17 +224,17 @@ def test_distance_approach_when_far():
def test_distance_retreat_clamped_when_too_close():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.radar_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 5.0 # Δd = -5 → kp 输出 -2.5 → 后退限幅 -1
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(-1.0, abs=1e-9)
def test_no_distance_hold_zeros_vx():
law = TrackingControlLaw(make_config()) # 缺省 kHold
law = TrackingControlLaw(make_config()) # 基准配置 HOLD
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.mode is ControlMode.VELOCITY_HEADING
assert out.vx_mps == 0.0
@@ -244,29 +245,79 @@ def test_no_distance_slow_approach():
cfg.distance.no_distance_action = NoDistanceAction.SLOW_APPROACH
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(0.5, abs=1e-9)
def test_no_distance_exit_degrades():
def test_no_distance_exit_waits_grace_then_degrades():
# S1 拍板：EXIT 需持续无距离达门限才降级；grace 期间按 SLOW_APPROACH 行为
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 200
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.associated_to_target = False
snap.distance_valid = False
out1 = law.update(snap, 1000)
assert out1.mode is ControlMode.VELOCITY_HEADING
assert out1.vx_mps == pytest.approx(0.5, abs=1e-9)
snap.track_time_ms = 1200 # 保持视觉/姿态新鲜，聚焦距离通道
snap.attitude_time_ms = 1200
out2 = law.update(snap, 1200) # 恰达门限 → 降级
assert out2.mode is ControlMode.DEGRADED_HOLD
assert out2.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
assert not out2.valid
def test_no_distance_exit_grace_resets_on_valid_frame():
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 200
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.distance_valid = False
assert law.update(snap, 1000).mode is ControlMode.VELOCITY_HEADING
snap.track_time_ms = 1100 # 有效距离帧：重置计时
snap.attitude_time_ms = 1100
snap.distance_valid = True
snap.target_distance_m = 30.0
snap.distance_time_ms = 1100
assert law.update(snap, 1100).mode is ControlMode.VELOCITY_HEADING
snap.track_time_ms = 1250 # 再次无效：重新计时
snap.attitude_time_ms = 1250
snap.distance_valid = False
snap.target_distance_m = 0.0
assert law.update(snap, 1250).mode is ControlMode.VELOCITY_HEADING # 0 ms < 200 ms
snap.track_time_ms = 1500
snap.attitude_time_ms = 1500
out = law.update(snap, 1500) # 250 ms ≥ 200 ms → 降级
assert out.mode is ControlMode.DEGRADED_HOLD
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
def test_no_distance_exit_zero_grace_degrades_immediately():
# grace = 0 保留旧"首个无效拍即退出"行为
cfg = make_config()
cfg.distance.no_distance_action = NoDistanceAction.EXIT
cfg.distance.no_distance_exit_grace_ms = 0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.distance_valid = False
out = law.update(snap, 1000)
assert out.mode is ControlMode.DEGRADED_HOLD
assert out.degraded_reason is DegradedReason.NO_DISTANCE_EXIT
assert not out.valid
def test_stale_radar_falls_back_to_no_distance():
law = TrackingControlLaw(make_config()) # kHold
def test_stale_distance_falls_back_to_no_distance():
law = TrackingControlLaw(make_config()) # HOLD
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 400 # 600 ms 前 > 500 ms 阈值 → 距离不可用
out = law.update(snap, 1000)
assert out.mode is ControlMode.VELOCITY_HEADING
assert out.vx_mps == 0.0
@@ -280,9 +331,9 @@ def test_first_tick_vx_slews_from_zero():
cfg.accel_limit.ax_mps2 = 2.0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0 # 通道输出本应为 3.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0 # 通道输出本应为 3.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000) # 首拍：dt 按 1/20 Hz 兜底
assert out.vx_mps == pytest.approx(2.0 * (1.0 / 20.0), abs=1e-9) # 0.1
@@ -293,9 +344,9 @@ def test_output_rotated_to_ned_by_yaw():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.yaw_rad = math.pi / 2.0 # 机头朝东
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
# 机体前向 3.0 → 东向 3.0，北向 ≈ 0
assert out.vx_mps == pytest.approx(0.0, abs=1e-9)
@@ -306,9 +357,9 @@ def test_zero_yaw_keeps_forward_as_north():
law = TrackingControlLaw(make_config())
snap = make_snapshot(1000)
snap.yaw_rad = 0.0
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
out = law.update(snap, 1000)
assert out.vx_mps == pytest.approx(3.0, abs=1e-9)
assert out.vy_mps == pytest.approx(0.0, abs=1e-9)
@@ -321,17 +372,17 @@ def test_reset_restores_slew_from_zero():
cfg.accel_limit.ax_mps2 = 2.0
law = TrackingControlLaw(cfg)
snap = make_snapshot(1000)
snap.radar_distance_m = 30.0
snap.associated_to_target = True
snap.radar_time_ms = 1000
snap.target_distance_m = 30.0
snap.distance_valid = True
snap.distance_time_ms = 1000
for t in range(1000, 3000, 50):
snap.track_time_ms = t
snap.radar_time_ms = t
snap.distance_time_ms = t
snap.attitude_time_ms = t
law.update(snap, t) # 若干拍后 vx 已爬升到限幅 3.0
law.reset()
snap.track_time_ms = 3000
snap.radar_time_ms = 3000
snap.distance_time_ms = 3000
snap.attitude_time_ms = 3000
out = law.update(snap, 3000)
# Reset 后限幅器归零：又从 0.1 开始渐变