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

utils/visual_track_control/config.py

@@ -23,18 +23,19 @@ class HeadingMode(Enum):
 
 
 class NoDistanceAction(Enum):
-    """雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3）。"""
+    """目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）。"""
 
     HOLD = "hold"                  # 保持：前向速度指令清零
     SLOW_APPROACH = "slow_approach"  # 限速接近：按 no_distance_approach_limit_mps 缓速
-    EXIT = "exit"                  # 退出近距操作：输出降级保持
+    EXIT = "exit"                  # 退出近距操作：持续无距离达 no_distance_exit_grace_ms
+                                   # 才降级保持；grace 期间按 SLOW_APPROACH 行为
 
 
 @dataclass
 class ControlGroupConfig:
     frequency_hz: float = 20.0
     visual_stale_ms: int = 200
-    radar_stale_ms: int = 500
+    distance_stale_ms: int = 500
     attitude_stale_ms: int = 300
 
 
@@ -70,8 +71,11 @@ class DistanceGroupConfig:
     derivative_filter_coef: float = 0.0
     approach_velocity_limit_mps: float = 3.0
     retreat_velocity_limit_mps: float = 1.0
-    no_distance_action: NoDistanceAction = NoDistanceAction.HOLD
+    # 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
+    # 持续无距离（传感器级故障）场景
+    no_distance_action: NoDistanceAction = NoDistanceAction.SLOW_APPROACH
     no_distance_approach_limit_mps: float = 0.5
+    no_distance_exit_grace_ms: int = 3000  # EXIT 门限：持续无距离达此时长才退出（毫秒，占位）
 
 
 @dataclass
@@ -155,9 +159,9 @@ def _load_control(root: dict) -> ControlGroupConfig:
     c.frequency_hz = _require_positive(
         _require_number(s, "control", "frequency_hz"), "control.frequency_hz")
     c.visual_stale_ms = _require_integer(s, "control", "visual_stale_ms")
-    c.radar_stale_ms = _require_integer(s, "control", "radar_stale_ms")
+    c.distance_stale_ms = _require_integer(s, "control", "distance_stale_ms")
     c.attitude_stale_ms = _require_integer(s, "control", "attitude_stale_ms")
-    if c.visual_stale_ms <= 0 or c.radar_stale_ms <= 0 or c.attitude_stale_ms <= 0:
+    if c.visual_stale_ms <= 0 or c.distance_stale_ms <= 0 or c.attitude_stale_ms <= 0:
         raise ValueError("配置非法: control.*_stale_ms 必须为正")
     return c
 
@@ -240,6 +244,10 @@ def _load_distance(root: dict) -> DistanceGroupConfig:
     d.no_distance_approach_limit_mps = _require_non_negative(
         _require_number(s, "distance", "no_distance_approach_limit_mps"),
         "distance.no_distance_approach_limit_mps")
+    # 0 容许：等效旧版"首个无效拍即退出"行为
+    d.no_distance_exit_grace_ms = _require_non_negative(
+        _require_integer(s, "distance", "no_distance_exit_grace_ms"),
+        "distance.no_distance_exit_grace_ms")
     return d