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

reference-c++/visual_track_control/config/track_control.json

@@ -4,7 +4,7 @@
     "control": {
         "frequency_hz": 20.0,
         "visual_stale_ms": 200,
-        "radar_stale_ms": 500,
+        "distance_stale_ms": 500,
         "attitude_stale_ms": 300
     },
 
@@ -36,8 +36,9 @@
         "derivative_filter_coef": 0.2,
         "approach_velocity_limit_mps": 3.0,
         "retreat_velocity_limit_mps": 1.0,
-        "no_distance_action": "hold",
-        "no_distance_approach_limit_mps": 0.5
+        "no_distance_action": "slow_approach",
+        "no_distance_approach_limit_mps": 0.5,
+        "no_distance_exit_grace_ms": 3000
     },
 
     "accel_limit": {

reference-c++/visual_track_control/examples/track_control_demo.cpp

@@ -179,11 +179,11 @@ int main(int argc, char** argv) {
         snapshot.track = tracker.LastResult();
         snapshot.track_time_ms = now_ms;
 
-        // 雷达：正常阶段关联成立；Coast/刹车阶段取消关联（演示 §6.3 降级路径）
-        snapshot.radar_distance_m = 40.0 - 3.0 * elapsed_s;
-        snapshot.associated_to_target = (tick < normal_ticks) &&
-                                        snapshot.radar_distance_m > 0.0;
-        snapshot.radar_time_ms = now_ms;
+        // 双目距离：正常阶段有效；Coast/刹车阶段置无效（演示 §6.3 降级路径）
+        snapshot.target_distance_m = 40.0 - 3.0 * elapsed_s;
+        snapshot.distance_valid = (tick < normal_ticks) &&
+                                  snapshot.target_distance_m > 0.0;
+        snapshot.distance_time_ms = now_ms;
 
         // 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
         snapshot.attitude_present = true;
@@ -209,11 +209,11 @@ int main(int argc, char** argv) {
         if (tick % 10 == 0) {
             spdlog::info(
                 "[t={:5.1f}s] 阶段={} 模式={}{} tracked={} vx={:+.2f} vy={:+.2f} "
-                "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 关联={}",
+                "vz={:+.2f} yaw_rate={:+.1f}dps d={:5.1f}m 有效={}",
                 elapsed_s, phase, ModeName(output.mode),
                 output.coast_active ? "(Coast)" : "", snapshot.track.tracked,
                 output.vx_mps, output.vy_mps, output.vz_mps, output.yaw_rate_dps,
-                snapshot.radar_distance_m, snapshot.associated_to_target);
+                snapshot.target_distance_m, snapshot.distance_valid);
         }
 
         std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));

reference-c++/visual_track_control/include/visual_track_control/tracking_control_law.h

@@ -6,7 +6,7 @@
 // 对应规格：docs/物理追踪思路.md
 //   §4  水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
 //   §5  垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
-//   §6  距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3）
+//   §6  距离控制（PID(Δd) → 前向速度；距离无效按配置降级，§6.3）
 //   §6.4 Coast 清积分、判丢零速刹车
 //   §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
 //   §8   统一限幅
@@ -58,6 +58,11 @@ private:
     // ---- 节拍与状态（仅打日志用，不参与数学） ----
     std::int64_t last_now_ms_ = 0;                       // 上次计算时刻
     ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志）
+
+    // ---- kExit 无距离计时（§6.3：持续无距离达 no_distance_exit_grace_ms
+    //      才退出，任一有效距离帧重置） ----
+    bool no_distance_active_ = false;        // 当前是否处于连续无距离状态
+    std::int64_t no_distance_since_ms_ = 0;  // 连续无距离起始时刻（单调时钟）
 };
 
 }  // namespace drone::vtc

reference-c++/visual_track_control/include/visual_track_control/vtc_config.h

@@ -33,18 +33,19 @@ enum class HeadingMode : std::uint8_t {
     kPosition,  // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
 };
 
-/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定）
+/// 目标距离不可用（无效/超龄）时的距离通道降级动作（§6.3）
 enum class NoDistanceAction : std::uint8_t {
     kHold,         // 保持：前向速度指令清零（安全占位语义）
     kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
-    kExit,         // 退出近距操作：输出降级保持（kDegradedHold）
+    kExit,         // 退出近距操作：持续无距离达 no_distance_exit_grace_ms 才
+                   // 降级保持（kDegradedHold）；grace 期间按 kSlowApproach 行为
 };
 
 /// control 组：控制周期与各输入超龄阈值（§7）
 struct ControlGroupConfig {
     double frequency_hz = 20.0;             // 控制频率（Hz，§7.1 待定）
     std::int64_t visual_stale_ms = 200;     // 视觉追踪结果超龄阈值（毫秒）
-    std::int64_t radar_stale_ms = 500;      // 雷达距离超龄阈值（毫秒）
+    std::int64_t distance_stale_ms = 500;   // 目标距离超龄阈值（毫秒）
     std::int64_t attitude_stale_ms = 300;   // 自机姿态超龄阈值（毫秒）
 };
 
@@ -84,8 +85,12 @@ struct DistanceGroupConfig {
     double derivative_filter_coef = 0.0;  // 微分一阶低通系数 ∈ [0,1)：0=不滤波
     double approach_velocity_limit_mps = 3.0;  // 接近速度限幅（米/秒，§8）
     double retreat_velocity_limit_mps = 1.0;   // 后退速度限幅（米/秒，§8）
-    NoDistanceAction no_distance_action = NoDistanceAction::kHold;  // 无距离降级（§6.3）
+    // 默认定速接近（D4 远场策略）：双目有效域外无距离是常态，退出仅留给
+    // 持续无距离（传感器级故障）场景
+    NoDistanceAction no_distance_action = NoDistanceAction::kSlowApproach;  // 无距离降级（§6.3）
     double no_distance_approach_limit_mps = 0.5;  // kSlowApproach 限速值（米/秒）
+    std::int64_t no_distance_exit_grace_ms = 3000;  // kExit 门限：持续无距离达
+                                                    // 此时长才退出（毫秒，占位）
 };
 
 /// accel_limit 组：各轴加速度限幅（§8，待定）

reference-c++/visual_track_control/include/visual_track_control/vtc_types.h

@@ -13,8 +13,8 @@
 //   - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
 //     控制律热路径零堆分配；
 //   - 本库不依赖主工程 include/ 的任何类型，独立可编译；
-//   - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作
-//     目标距离（§6.3、需求分析.md FR-041）。
+//   - 目标距离必须带 distance_valid 有效标志：无效值不得当作目标距离
+//     （§6.3、需求分析.md FR-041）。
 // =============================================================================
 
 #include <cstdint>
@@ -54,10 +54,11 @@ struct ControlSnapshot {
     drone::tracker::TrackResult track;  // 追踪库结果（值拷贝）
     std::int64_t track_time_ms = 0;     // 取样单调时钟（毫秒），<=0 视为从未取样
 
-    // ---- 雷达距离（§6.3：未关联仅为前向安全距离，不得当目标距离） ----
-    double radar_distance_m = 0.0;      // 机头前向单点距离（米）
-    bool associated_to_target = false;  // 相机—雷达空间关联成立标志
-    std::int64_t radar_time_ms = 0;     // 取样单调时钟（毫秒）
+    // ---- 目标距离（双目立体测距，§6.3：distance_valid == false 时不得
+    //      当作目标距离） ----
+    double target_distance_m = 0.0;     // 目标距离（米，双目前向分量 Z）
+    bool distance_valid = false;        // 距离有效标志（测距来自检测框自身，无跨模态关联）
+    std::int64_t distance_time_ms = 0;  // 取样单调时钟（毫秒）
 
     // ---- 自机姿态/速度 ----
     // 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转

reference-c++/visual_track_control/src/tracking_control_law.cpp

@@ -9,7 +9,8 @@
 //             消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
 //             vtc_types.h ControlMode 注释）
 //   §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）；
-//             未关联/超龄 → 按配置降级动作（hold / slow_approach / exit）
+//             无效/超龄 → 按配置降级动作（hold / slow_approach / exit，
+//             exit 需持续无距离达 no_distance_exit_grace_ms 门限）
 //   §6.4      Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车
 //   §7.1      超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
 //   §8        所有输出统一限幅；首条指令经变化率限幅（S6）
@@ -123,6 +124,7 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
 
     // ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
     if (!snapshot.track.tracked) {
+        no_distance_active_ = false;  // 刹车期间不计 kExit 无距离时长
         if (last_mode_ != ControlMode::kBrakeHover) {
             // 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
             distance_pid_.Reset();
@@ -181,14 +183,16 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
     }
 
     // ---- ⑦ 距离通道（§6.2 / §6.3） ----
-    // 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§6.3、FR-041）
+    // 目标距离来自检测框自身的双目测距：仅 distance_valid 且未超龄才可用
+    // （§6.3、FR-041）
     const bool distance_available =
-        snapshot.associated_to_target && snapshot.radar_distance_m > 0.0 &&
-        !StalenessChecker::IsStale(snapshot.radar_time_ms, now_ms,
-                                   cfg_.control.radar_stale_ms);
+        snapshot.distance_valid && snapshot.target_distance_m > 0.0 &&
+        !StalenessChecker::IsStale(snapshot.distance_time_ms, now_ms,
+                                   cfg_.control.distance_stale_ms);
     if (distance_available) {
+        no_distance_active_ = false;  // 任一有效距离帧重置 kExit 计时
         // Δd = d − d_exp：d > d_exp 前向接近，d < d_exp 减速/后退（§6.2）
-        const double delta_d = snapshot.radar_distance_m - cfg_.distance.d_exp_m;
+        const double delta_d = snapshot.target_distance_m - cfg_.distance.d_exp_m;
         const double vx_raw = distance_pid_.Update(delta_d, dt_s);
         const double slewed = vx_limiter_.Limit(vx_raw, dt_s);  // 加速度限幅（§8）
         out.vx_mps = Clamp(slewed, -cfg_.distance.retreat_velocity_limit_mps,
@@ -208,10 +212,24 @@ ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
                     cfg_.distance.no_distance_approach_limit_mps, dt_s);
                 break;
             case NoDistanceAction::kExit:
-                // 退出近距操作：整体降级保持
-                out.mode = ControlMode::kDegradedHold;
-                out.degraded_reason = DegradedReason::kNoDistanceExit;
-                return FinishAndLog("距离未关联，退出近距操作");
+                // 退出近距操作：双目有效域外无距离是常态（D4 远场定速接近），
+                // 首个无效拍不退出；持续无距离达 no_distance_exit_grace_ms
+                // （疑似传感器级故障）才降级。grace 期间按 kSlowApproach 行为
+                // 执行（经加速度限幅，vx 平滑过渡）；grace=0 保留旧"立即退出"
+                // 行为
+                if (!no_distance_active_) {
+                    no_distance_active_ = true;
+                    no_distance_since_ms_ = now_ms;
+                }
+                if (now_ms - no_distance_since_ms_ >=
+                    cfg_.distance.no_distance_exit_grace_ms) {
+                    out.mode = ControlMode::kDegradedHold;
+                    out.degraded_reason = DegradedReason::kNoDistanceExit;
+                    return FinishAndLog("持续无距离达门限，退出近距操作");
+                }
+                out.vx_mps = vx_limiter_.Limit(
+                    cfg_.distance.no_distance_approach_limit_mps, dt_s);
+                break;
         }
     }
 
@@ -232,6 +250,7 @@ void TrackingControlLaw::Reset() {
     yaw_rate_limiter_.Reset();
     vx_limiter_.Reset();
     vz_limiter_.Reset();
+    no_distance_active_ = false;
     last_now_ms_ = 0;
     last_mode_ = ControlMode::kDegradedHold;
 }

reference-c++/visual_track_control/src/vtc_config.cpp

@@ -107,9 +107,9 @@ ControlGroupConfig LoadControl(const Json& root) {
     c.frequency_hz = RequirePositive(RequireNumber(s, "control", "frequency_hz"),
                                      "control.frequency_hz");
     c.visual_stale_ms = RequireInteger(s, "control", "visual_stale_ms");
-    c.radar_stale_ms = RequireInteger(s, "control", "radar_stale_ms");
+    c.distance_stale_ms = RequireInteger(s, "control", "distance_stale_ms");
     c.attitude_stale_ms = RequireInteger(s, "control", "attitude_stale_ms");
-    if (c.visual_stale_ms <= 0 || c.radar_stale_ms <= 0 || c.attitude_stale_ms <= 0) {
+    if (c.visual_stale_ms <= 0 || c.distance_stale_ms <= 0 || c.attitude_stale_ms <= 0) {
         throw std::runtime_error("配置非法: control.*_stale_ms 必须为正");
     }
     return c;
@@ -193,6 +193,13 @@ DistanceGroupConfig LoadDistance(const Json& root) {
     d.no_distance_approach_limit_mps = RequireNonNegative(
         RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
         "distance.no_distance_approach_limit_mps");
+    // 0 容许：等效旧版"首个无效拍即退出"行为
+    d.no_distance_exit_grace_ms = RequireInteger(s, "distance",
+                                                 "no_distance_exit_grace_ms");
+    if (d.no_distance_exit_grace_ms < 0) {
+        throw std::runtime_error(
+            "配置非法(不得为负): distance.no_distance_exit_grace_ms");
+    }
     return d;
 }
 

reference-c++/visual_track_control/tests/tracking_control_law_test.cpp

@@ -4,8 +4,9 @@
 // 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
 //   超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
 //   Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
-//   A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
-//   首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
+//   A2）、垂直统一速度支路（A1）、距离通道无距离降级动作（含 kExit
+//   门限/grace 重置/零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、
+//   机体系→NED 输出旋转（A4）。
 // =============================================================================
 
 #include <cmath>
@@ -31,7 +32,7 @@ TrackingConfig MakeConfig() {
     TrackingConfig cfg;
     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;
@@ -59,7 +60,7 @@ TrackingConfig MakeConfig() {
     return cfg;
 }
 
-/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜
+/// 新鲜快照：目标在画面中心，姿态/追踪/距离均新鲜
 ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
     ControlSnapshot s;
     s.track.tracked = true;
@@ -220,9 +221,9 @@ TEST(TrackingControlLawTest, VerticalVelocityClamped) {
 TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
     TrackingControlLaw law(MakeConfig());
     auto snap = MakeSnapshot(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;
     const auto out = law.Update(snap, 1000);
     EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
 }
@@ -230,9 +231,9 @@ TEST(TrackingControlLawTest, DistanceApproachWhenFar) {
 TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
     TrackingControlLaw law(MakeConfig());
     auto snap = MakeSnapshot(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;
     const auto out = law.Update(snap, 1000);
     EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
 }
@@ -240,7 +241,7 @@ TEST(TrackingControlLawTest, DistanceRetreatClampedWhenTooClose) {
 TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
     TrackingControlLaw law(MakeConfig());  // 缺省 kHold
     auto snap = MakeSnapshot(1000);
-    snap.associated_to_target = false;
+    snap.distance_valid = false;
     const auto out = law.Update(snap, 1000);
     EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
     EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
@@ -251,29 +252,80 @@ TEST(TrackingControlLawTest, NoDistanceSlowApproach) {
     cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
     TrackingControlLaw law(cfg);
     auto snap = MakeSnapshot(1000);
-    snap.associated_to_target = false;
+    snap.distance_valid = false;
     const auto out = law.Update(snap, 1000);
     EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
 }
 
-TEST(TrackingControlLawTest, NoDistanceExitDegrades) {
+TEST(TrackingControlLawTest, NoDistanceExitWaitsGraceThenDegrades) {
+    // S1 拍板：kExit 需持续无距离达门限才降级；grace 期间按 kSlowApproach 行为
     auto cfg = MakeConfig();
     cfg.distance.no_distance_action = NoDistanceAction::kExit;
+    cfg.distance.no_distance_exit_grace_ms = 200;
     TrackingControlLaw law(cfg);
     auto snap = MakeSnapshot(1000);
-    snap.associated_to_target = false;
+    snap.distance_valid = false;
+    const auto out1 = law.Update(snap, 1000);
+    EXPECT_EQ(out1.mode, ControlMode::kVelocityHeading);
+    EXPECT_NEAR(out1.vx_mps, 0.5, 1e-9);
+    snap.track_time_ms = 1200;  // 保持视觉/姿态新鲜，聚焦距离通道
+    snap.attitude_time_ms = 1200;
+    const auto out2 = law.Update(snap, 1200);  // 恰达门限 → 降级
+    EXPECT_EQ(out2.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out2.degraded_reason, DegradedReason::kNoDistanceExit);
+    EXPECT_FALSE(out2.valid);
+}
+
+TEST(TrackingControlLawTest, NoDistanceExitGraceResetsOnValidFrame) {
+    auto cfg = MakeConfig();
+    cfg.distance.no_distance_action = NoDistanceAction::kExit;
+    cfg.distance.no_distance_exit_grace_ms = 200;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.distance_valid = false;
+    EXPECT_EQ(law.Update(snap, 1000).mode, ControlMode::kVelocityHeading);
+
+    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;
+    EXPECT_EQ(law.Update(snap, 1100).mode, ControlMode::kVelocityHeading);
+
+    snap.track_time_ms = 1250;  // 再次无效：重新计时
+    snap.attitude_time_ms = 1250;
+    snap.distance_valid = false;
+    snap.target_distance_m = 0.0;
+    EXPECT_EQ(law.Update(snap, 1250).mode,
+              ControlMode::kVelocityHeading);  // 0 ms < 200 ms 门限
+
+    snap.track_time_ms = 1500;
+    snap.attitude_time_ms = 1500;
+    const auto out = law.Update(snap, 1500);  // 250 ms ≥ 200 ms → 降级
+    EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
+    EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
+}
+
+TEST(TrackingControlLawTest, NoDistanceExitZeroGraceDegradesImmediately) {
+    // grace = 0 保留旧"首个无效拍即退出"行为
+    auto cfg = MakeConfig();
+    cfg.distance.no_distance_action = NoDistanceAction::kExit;
+    cfg.distance.no_distance_exit_grace_ms = 0;
+    TrackingControlLaw law(cfg);
+    auto snap = MakeSnapshot(1000);
+    snap.distance_valid = false;
     const auto out = law.Update(snap, 1000);
     EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
     EXPECT_EQ(out.degraded_reason, DegradedReason::kNoDistanceExit);
     EXPECT_FALSE(out.valid);
 }
 
-TEST(TrackingControlLawTest, StaleRadarFallsBackToNoDistance) {
+TEST(TrackingControlLawTest, StaleDistanceFallsBackToNoDistance) {
     TrackingControlLaw law(MakeConfig());  // kHold
     auto snap = MakeSnapshot(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 阈值 → 距离不可用
     const auto out = law.Update(snap, 1000);
     EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
     EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
@@ -287,9 +339,9 @@ TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) {
     cfg.accel_limit.ax_mps2 = 2.0;
     TrackingControlLaw law(cfg);
     auto snap = MakeSnapshot(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;
     const auto out = law.Update(snap, 1000);  // 首拍：dt 按 1/20 Hz 兜底
     EXPECT_NEAR(out.vx_mps, 2.0 * (1.0 / 20.0), 1e-9);  // 0.1
 }
@@ -300,9 +352,9 @@ TEST(TrackingControlLawTest, OutputRotatedToNedByYaw) {
     TrackingControlLaw law(MakeConfig());
     auto snap = MakeSnapshot(1000);
     snap.yaw_rad = kPi / 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;
     const auto out = law.Update(snap, 1000);
     // 机体前向 3.0 → 东向 3.0，北向 ≈ 0
     EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
@@ -313,9 +365,9 @@ TEST(TrackingControlLawTest, ZeroYawKeepsForwardAsNorth) {
     TrackingControlLaw law(MakeConfig());
     auto snap = MakeSnapshot(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;
     const auto out = law.Update(snap, 1000);
     EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
     EXPECT_NEAR(out.vy_mps, 0.0, 1e-9);
@@ -328,18 +380,18 @@ TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) {
     cfg.accel_limit.ax_mps2 = 2.0;
     TrackingControlLaw law(cfg);
     auto snap = MakeSnapshot(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 (std::int64_t t = 1000; t < 3000; t += 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;
     const auto out = law.Update(snap, 3000);
     // Reset 后限幅器归零：又从 0.1 开始渐变

reference-c++/visual_track_control/tests/vtc_config_test.cpp

@@ -30,7 +30,7 @@ const char* kFullJson = R"({
     "control": {
         "frequency_hz": 20.0,
         "visual_stale_ms": 200,
-        "radar_stale_ms": 500,
+        "distance_stale_ms": 500,
         "attitude_stale_ms": 300
     },
     "camera": {
@@ -59,7 +59,8 @@ const char* kFullJson = R"({
         "approach_velocity_limit_mps": 3.0,
         "retreat_velocity_limit_mps": 1.0,
         "no_distance_action": "hold",
-        "no_distance_approach_limit_mps": 0.5
+        "no_distance_approach_limit_mps": 0.5,
+        "no_distance_exit_grace_ms": 3000
     },
     "accel_limit": {
         "ax_mps2": 2.0,
@@ -96,7 +97,9 @@ TEST(VtcConfigTest, LoadRealConfigSucceeds) {
     EXPECT_DOUBLE_EQ(cfg.vertical.vz_gain_mps_per_deg, 0.1);
     EXPECT_DOUBLE_EQ(cfg.vertical.vz_limit_mps, 2.0);
     EXPECT_DOUBLE_EQ(cfg.distance.d_exp_m, 10.0);
-    EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kHold);
+    EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kSlowApproach);
+    EXPECT_EQ(cfg.distance.no_distance_exit_grace_ms, 3000);
+    EXPECT_EQ(cfg.control.distance_stale_ms, 500);
     EXPECT_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0);
 }
 
@@ -221,4 +224,25 @@ TEST(VtcConfigTest, FovOutOfRangeThrows) {
     std::remove(path.c_str());
 }
 
+TEST(VtcConfigTest, DefaultNoDistanceActionIsSlowApproach) {
+    // S1 拍板：双目有效域外无距离是常态，默认定速接近（纪要 D4/S1）
+    const TrackingConfig cfg;
+    EXPECT_EQ(cfg.distance.no_distance_action, NoDistanceAction::kSlowApproach);
+}
+
+TEST(VtcConfigTest, NegativeExitGraceThrows) {
+    std::string json = kFullJson;
+    ReplaceOnce(json, "\"no_distance_exit_grace_ms\": 3000",
+                "\"no_distance_exit_grace_ms\": -1");
+    const std::string path = WriteTempJson(json, "vtc_cfg_badgrace.json");
+    try {
+        TrackingConfig::LoadFromJson(path);
+        FAIL() << "应当抛出异常";
+    } catch (const std::runtime_error& e) {
+        EXPECT_NE(std::string(e.what()).find("no_distance_exit_grace_ms"),
+                  std::string::npos);
+    }
+    std::remove(path.c_str());
+}
+
 }  // namespace

reference-c++/visual_track_control/视觉跟踪控制律实现文档.md

@@ -34,8 +34,9 @@
     垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消，
     原因见 `vtc_types.h` 的 `ControlMode` 注释与 §3.8）；
   - 距离通道（规格 §6）：`Δd = d − d_exp` → `PidController` → 前向速度
-    `vx_mps`；雷达未关联/超龄时按 `NoDistanceAction` 配置降级
-    （hold / slow_approach / exit）。
+    `vx_mps`；目标距离无效/超龄时按 `NoDistanceAction` 配置降级
+    （hold / slow_approach / exit，exit 需持续无距离达
+    `no_distance_exit_grace_ms` 门限，见 §3.8）。
 - **统一限幅**（规格 §8）：所有输出通道内联完成限幅——`Clamp` 静态限幅 +
   `SlewRateLimiter` 变化率限幅（航向修正/角速度、前向/垂直加速度），
   全部在机体系完成。
@@ -96,7 +97,8 @@
 （原 `kAttitudeTrim` 姿态微调模式已取消，见 §3.8。）
 
 `enum class DegradedReason`：`kNone` / `kAttitudeStale`（姿态过期）/
-`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（无距离且配置退出近距操作）。
+`kVisionStale`（视觉超龄）/ `kNoDistanceExit`（持续无距离达门限且配置
+退出近距操作）。
 
 `ControlSnapshot`（输入，纯值类型）：
 
@@ -104,9 +106,9 @@
 |------|------|
 | `track` | `drone::tracker::TrackResult` 值拷贝（追踪库主目标输出，字段定义见 `../target_tracker/target_tracker.md` §二） |
 | `track_time_ms` | 追踪结果取样单调时钟（毫秒）；**<=0 视为从未取样，直接判超龄** |
-| `radar_distance_m` | 机头前向单点距离（米） |
-| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
-| `radar_time_ms` | 雷达取样单调时钟 |
+| `target_distance_m` | 目标距离（米，双目立体测距前向分量 Z） |
+| `distance_valid` | 距离有效标志；**无效不得当目标距离**（规格 §6.3、需求分析 FR-041）。距离来自目标检测框自身的双目测距，无跨模态关联问题 |
+| `distance_time_ms` | 距离取样单调时钟 |
 | `attitude_present` / `attitude_time_ms` | 姿态源可用性与取样时刻（过期→禁止控制，规格 §9.2） |
 | `yaw_rad` | 自机航向（弧度，NED）；**参与计算**：输出端机体系→NED 旋转（§2.3）与位置式绝对航向合成 |
 | `roll_rad` / `pitch_rad` / `yaw_rate_radps` | 自机姿态其余分量（弧度，FRD）；预留，机体补偿未实现（规格 §4.1） |
@@ -124,15 +126,18 @@
 | `coast_active` | 本周期处于 Coast（`is_predicted == true`） |
 
 `TrackingConfig` 六分组：`control`（`frequency_hz`、`visual_stale_ms`、
-`radar_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
+`distance_stale_ms`、`attitude_stale_ms`）、`camera`（`image_width`、
 `image_height`、`fov_h_deg`、`fov_v_deg`）、`heading`（`mode`、`gain`、
 `yaw_rate_limit_dps`、`yaw_slew_limit_dps2`）、`vertical`
 （`vz_gain_mps_per_deg`、`vz_limit_mps`）、`distance`（`d_exp_m`、
 `kp`/`ki`/`kd`、`integral_limit`、`derivative_filter_coef`、
 `approach_velocity_limit_mps`、`retreat_velocity_limit_mps`、
-`no_distance_action`、`no_distance_approach_limit_mps`）、`accel_limit`
+`no_distance_action`、`no_distance_approach_limit_mps`、
+`no_distance_exit_grace_ms`）、`accel_limit`
 （`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
 `config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
+`distance.no_distance_action` 默认 `slow_approach`（双目有效域外无距离
+是常态，对齐 D4 远场定速接近策略，见《双目测距方案讨论纪要》S1）；
 相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
 
 ### 2.3 ICommandTransport 契约
@@ -263,11 +268,11 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
 → ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分
 → ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 `HeadingMode` 分支）
 → ⑥ 垂直统一速度支路（α → `vz_gain·α` → 加速度限幅 → 速度限幅）
-→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
+→ ⑦ 距离通道（`distance_valid` 且未超龄才可用；否则清积分并按
 `no_distance_action` 降级）→ ⑧ 机体系→NED 旋转（`RotateBodyToNed`，
 随 `snapshot.yaw_rad`）→ 置 `valid = true` 输出。
 
-两个设计决策记录：
+三个设计决策记录：
 
 - **垂直双支路合并（A1）**：规格 §5.2 原设"小误差姿态微调（`kAttitudeTrim`
   → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态
@@ -278,6 +283,14 @@ dt 上限 `kMaxDtS = 0.5` 秒）：
   航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。
   位置式分支输出 `WrapDeg180(当前航向 + 限幅修正量)`；速率式
   （默认）输出角速度指令，天然相对量、无此问题。
+- **kExit 持续无距离门限（纪要 S1）**：换双目后有效域外无距离是常态
+  （纪要 D4 远场定速接近），首个无效拍即退出会频繁降级。`kExit` 现需
+  连续无距离达 `no_distance_exit_grace_ms` 才降级（语义收敛为"疑似
+  传感器级故障"），grace 期间按 `kSlowApproach` 行为执行（经加速度
+  限幅，vx 平滑过渡），任一有效距离帧重置计时；`no_distance_exit_grace_ms
+  = 0` 保留旧"立即退出"行为。`no_distance_action` 默认值随之
+  `kHold` → `kSlowApproach`。判丢刹车期间不计无距离时长，`Reset()`
+  清计时状态。
 
 ### 3.9 MAVLink 消息映射（规格 §2.2）
 
@@ -347,12 +360,12 @@ cmake --build build_vtc -j$(nproc)
 `LoggingTransport` 字节打印），按配置频率固定节拍依次走三阶段
 （对应规格 §6.4）：
 
-1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，雷达
-   关联成立，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
+1. **正常跟踪（6 秒）**：持续喂入向画面中心漂移的合成检测框，双目
+   距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限，
    角速度随 β 收敛而减小）；
 2. **Coast（4 秒）**：停止喂检测 → `is_predicted == true` → 清积分、
-   继续预测跟踪；同时取消雷达关联，演示 §6.3 无距离降级路径（hold：
-   前向清零）；
+   继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为
+   `slow_approach`：按限速值缓速前向）；
 3. **判丢刹车（3 秒）**：连续丢帧超过追踪阈值 → `tracked == false` →
    零速刹车。
 
@@ -372,8 +385,8 @@ cmake --build build_vtc -j$(nproc)
 | `frame_transform_test.cpp` | ψ=0/90° 基向量映射、合速度守恒（正交性）、`v_down` 不变 |
 | `line_of_sight_test.cpp` | 零偏移、半幅=半视场、奇对称、单调性、与线性近似的偏离（证明未走线性支路）、构造期非法参数抛异常 |
 | `pid_controller_test.cpp` | P/I/D 分量、积分钳位、输出限幅、抗饱和冻结、**误差反号回卷回归**（旧判据钉在 +5，条件积分到 -5）、微分低通、`dt<=0`、`ResetIntegral`/`Reset` |
-| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
-| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
+| `tracking_control_law_test.cpp` | 姿态缺失/过期、视觉超龄、从未取样、姿态比视觉更严的优先级、判丢刹车、Coast 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 kExit 门限、grace 有效帧重置、零门限立即退出）、距离超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
+| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、默认降级动作为 slow_approach、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界/grace 为负均抛异常且消息携带字段名 |
 | `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
 
 运行方式（构建后）：
@@ -430,8 +443,11 @@ CMake config 包）；未找到时自动跳过测试构建并打印状态，不
    由接线部件完成 `TrackResult` → `TrackedTarget` 转换与控制线程订阅；
 3. **传输实现替换**：把示例的 `LoggingTransport` 换成真实链路实现
    （`ICommandTransport` 注入），打包与控制律零改动；
-4. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
-   规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
+4. **距离有效标志来源**：`distance_valid` 由双目链路（StereoRanger）
+   给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下
+   调用方填 `ControlSnapshot` 时应校验 `StereoTargetDistance.track_id`
+   与 `TrackResult.track_id` 一致（v1.0 单目标为调用方约定，纪要 S3，
+   库内不加代码）。
 
 ### 6.4 红线声明