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
reference-c++/visual_track_control/tests/tracking_control_law_test.cpp
+84 -32
View File
@@ -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
+27 -3
View File
@@ -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