songhaoyu/code-test-drone
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
code-test-drone/reference-c++/visual_track_control/视觉跟踪控制律实现文档.md
T

28 KiB
Raw Permalink Blame History

视觉跟踪控制律实现文档（visual_track_control）

独立静态库子工程，实现 ../docs/物理追踪思路.md（下称"规格"）定义的 视觉跟踪阶段（S7 VISUAL_TRACKING）控制律：水平航向、垂直速度、 距离 PID 三路控制 + 统一限幅 + Coast 清积分 / 判丢刹车 + 超龄降级， 输出端随机体航向完成机体系→局部 NED 旋转，经 MavlinkCommandGateway 打包为 MAVLink 字节流，交由注入的 ICommandTransport 实现送达。

本文档按 ../AGENTS.md"实现文档规范"六要素组织。三份文档分工明确、 互不重复：

  • ../docs/物理追踪思路.md——控制律物理与行为规格（唯一权威）， 本文档不重复抄录公式推导；
  • ../docs/视觉跟踪控制律技术路线.md——语言无关实现路线：单周期八步 伪代码、构件与状态清单、坐标系与符号约定、打包掩码取舍、重写指引；
  • 本文档——C++ 落地说明：公共头一览、构建与测试方式、日志行为、 排查与修改要点。

追踪库行为约定见 ../target_tracker/target_tracker.md，同样只引用不复述。

一、功能职责与边界

命名空间 drone::vtc，头文件保护一律 #pragma once。

做什么：

  • 三路控制律（TrackingControlLaw::Update() 单周期计算）：
    • 水平航向（规格 §4）：像素偏移 → 水平视线角 β → 增益 → 变化率限幅 → 角速度限幅，按 HeadingMode 配置输出速率式 yaw_rate_dps（默认） 或位置式 yaw_deg（绝对 NED 航向，见 §3.8）；
    • 垂直速度（规格 §5）：垂直视线角 α → 统一速度支路 → vz_mps 垂直速度设定值（原"小误差姿态微调/大误差垂直速度"双支路已取消， 原因见 vtc_types.h 的 ControlMode 注释与 §3.8）；
    • 距离通道（规格 §6）：Δd = d − d_exp → PidController → 前向速度 vx_mps；目标距离无效/超龄时按 NoDistanceAction 配置降级 （hold / slow_approach / exit，exit 需持续无距离达 no_distance_exit_grace_ms 门限，见 §3.8）。
  • 统一限幅（规格 §8）：所有输出通道内联完成限幅——Clamp 静态限幅 + SlewRateLimiter 变化率限幅（航向修正/角速度、前向/垂直加速度）， 全部在机体系完成。
  • 坐标系旋转（规格 §2.3）：输出端用 RotateBodyToNed() 把机体系 速度（前/右/下）随快照航向 ψ 旋转为局部 NED（北/东/地），与网关的 MAV_FRAME_LOCAL_NED 一致；仅绕下轴旋转（滚转/俯仰对速度指令为 二阶小量，由飞控内环处理）。
  • Coast 清积分（规格 §6.4）：is_predicted == true 时清空距离 PID 积分项（ResetIntegral()），防积分饱和。
  • 判丢刹车（规格 §6.4）：tracked == false 时输出 kBrakeHover 零速刹车，并清积分与全部限幅器历史（保证恢复跟踪后指令从零渐变）。
  • 超龄降级（规格 §7.1、§9.2）：姿态过期/缺失 → kDegradedHold （kAttitudeStale，禁止控制）；视觉结果超龄 → kDegradedHold （kVisionStale）；判定器为无状态 StalenessChecker::IsStale()。
  • MAVLink 打包（规格 §2.2）：MavlinkCommandGateway 把 ControlOutput 打包为 SET_POSITION_TARGET_LOCAL_NED（ID 84）字节流。
  • 配置化（规格 §8、需求分析 NFR-007）：全部阈值集中在 TrackingConfig，由 TrackingConfig::LoadFromJson() 启动期一次性加载， 代码不写死任何参数。

不做什么（边界）：

  • 不做感知：不运行检测/光流/雷达驱动，只消费调用方填入的 ControlSnapshot；
  • 不做状态机仲裁：S6/S8 切换、目标丢失策略（ITargetLostStrategy）、 障碍接管均在状态机层，本库只按输入标志产生当拍输出；
  • 不写串口/传输：库内无任何链路代码，字节流交给 ICommandTransport 实现方；
  • 不解析 MAVLink 输入：只有打包发送方向，无 mavlink_parse_*；
  • 不做机体补偿：ControlSnapshot 的 roll_rad / pitch_rad / yaw_rate_radps / velocity_* 字段为预留输入（规格 §4.1）；当前仅 yaw_rad 参与计算（NED 旋转与位置式航向合成）；
  • 不做热更新：配置启动期加载一次，调参重启生效。

二、接口与数据流

2.1 公共头一览

头文件 内容
include/visual_track_control/vtc_types.h ControlMode、DegradedReason、ControlSnapshot、ControlOutput
include/visual_track_control/vtc_config.h HeadingMode、NoDistanceAction、各分组配置结构、TrackingConfig
include/visual_track_control/tracking_control_law.h TrackingControlLaw（主控律）
include/visual_track_control/pid_controller.h PidParams、PidController
include/visual_track_control/rate_limiter.h Clamp()、SlewRateLimiter（header-only）
include/visual_track_control/staleness_checker.h StalenessChecker（header-only）
include/visual_track_control/line_of_sight.h PixelToAngle（像素→视线角，全程针孔模型）
include/visual_track_control/frame_transform.h RotateBodyToNed()（机体系→局部 NED，header-only）
include/visual_track_control/i_command_transport.h ICommandTransport（传输抽象）
include/visual_track_control/mavlink_command_gateway.h MavlinkCommandGateway（MAVLink 打包）

2.2 关键字段（以代码为准）

enum class ControlMode：kVelocityHeading（速度航向主输出）/ kBrakeHover（零速刹车）/ kDegradedHold（降级保持，禁止正常控制）。 （原 kAttitudeTrim 姿态微调模式已取消，见 §3.8。）

enum class DegradedReason：kNone / kAttitudeStale（姿态过期）/ kVisionStale（视觉超龄）/ kNoDistanceExit（持续无距离达门限且配置 退出近距操作）。

ControlSnapshot（输入，纯值类型）：

字段 含义
track drone::tracker::TrackResult 值拷贝（追踪库主目标输出，字段定义见 ../target_tracker/target_tracker.md §二）
track_time_ms 追踪结果取样单调时钟（毫秒）；<=0 视为从未取样，直接判超龄
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）
velocity_x_mps / velocity_y_mps / velocity_z_mps 自机速度分量（预留，接入主工程时由 FlightStateSnapshot 填充）

ControlOutput（输出，已限幅）：

字段 含义
mode / valid 输出模式 / 指令是否可用于发送
vx_mps / vy_mps / vz_mps 局部 NED 速度分量（北/东/地，米/秒）：各通道在机体系计算并限幅后，输出端随 snapshot.yaw_rad 旋转得到；旋转为正交变换，合速度大小不变。机体系侧向恒为 0，旋转后 vy_mps 可非零
yaw_deg 位置式目标绝对航向（度，NED，右转为正）：由当前航向 + 变化率限幅后的修正量合成，回绕到 [-180, 180)（ID 84 的 yaw 字段为绝对值）
yaw_rate_dps 速率式偏航角速度指令（度/秒，右转为正；天然是相对量，无坐标系歧义）
degraded_reason 降级原因（仅 kDegradedHold 有意义）
coast_active 本周期处于 Coast（is_predicted == true）

TrackingConfig 六分组：control（frequency_hz、visual_stale_ms、 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、 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 契约

class ICommandTransport {
public:
    virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
};
  • 只吃字节流：串口、电台、回环打印均可注入实现；后续重写传输层 只换实现，不动打包与控制律；
  • 返回 true = 已交付底层链路，false = 发送失败（调用方可计数/告警）；
  • 实现方保证不长期持有 data 指针（同步拷贝或同步写出）。

2.4 数据流向

推理侧每帧: TargetTracker::Update(detections, frame_shape)
控制线程(固定周期, 规格 §7.1):
    TargetTracker::LastResult()          # 线程安全拷贝
    → 取样瞬间用单调时钟打戳填入 ControlSnapshot.track_time_ms
    → TrackingControlLaw::Update(snapshot, now_ms)   # 单周期计算
    → 按 ControlOutput.mode 分发:
        kVelocityHeading → MavlinkCommandGateway::SendVelocityHeading()
        kBrakeHover      → MavlinkCommandGateway::SendBrakeHover()
        kDegradedHold    → 不发送（仅告警/降级）
    → mavlink_msg_to_send_buffer → ICommandTransport::Send(字节流)
停止视觉闭环: TrackingControlLaw::Reset()（规格 §6.4）

三、关键实现点

3.1 传输接口只吃字节流的双重解耦

打包（MavlinkCommandGateway）与传输（ICommandTransport）分离： 网关只负责 ControlOutput → MAVLink 字节，不感知链路形态；传输实现 只感知字节，不感知消息语义。对应规格 §2.2 控制单出口约束在独立库中的 映射——控制律不直接写飞控。

3.2 TrackResult 无时间戳，由调用方取样打戳

TrackResult 是追踪库自有类型，本身不含时间戳（规格 §7.2）。约定： 调用方在读取 LastResult() 的瞬间用单调时钟打戳填入 track_time_ms；超龄判定一律用"取样时间 + valid_for_ms" （StalenessChecker::IsStale(sample_time_ms, now_ms, valid_for_ms)）：

  • sample_time_ms <= 0 视为从未取样，直接判超龄；
  • now_ms < sample_time_ms（时间戳不一致：时钟源错配/打戳错误）同样 判超龄——单一单调时钟下不应发生，宁可降级也不信任不可核实的取样。

3.3 PID anti-windup 策略（条件积分）

PidController::Update()（位置式）三层防护：

  1. 积分钳位：候选积分值先钳制在 ±integral_limit；
  2. anti-windup（条件积分）：输出饱和（|raw| > output_limit）且 本拍积分增量仍在推深饱和（增量 error·dt 与原始输出 raw 同号）时，冻结本步积分累加（不采纳候选积分值）；误差反号后增量反向， 积分立即恢复回卷，无饱和残留。判据刻意基于"增量方向"而非"积分项与 输出同向"：后者在误差反号后仍会冻结回卷；也不用 raw·error 的 变体含混表述，实现即 (error*dt_s) * raw > 0；
  3. 输出限幅：最终输出钳制在 ±output_limit。

微分带一阶低通（derivative_filter_coef ∈ [0,1)，0 = 不滤波）； dt_s <= 0 时保持上次输出（异常节拍保护）。ResetIntegral() 只清积分， 保留微分历史与上次输出；Reset() 清全部，均 O(1)（Coast 清积分， 规格 §6.4）。

3.4 SlewRateLimiter 兼作首条指令突变抑制

SlewRateLimiter 保存上次输出，单步变化不超过 rate_limit · dt_s； 构造时初值置 0，天然实现规格要求的"首条视觉速度设定值必须经过限幅" （状态机设计 S6）：切换进入视觉跟踪后首条指令从零按加速度限幅渐变， 无需专门的切换分支。判丢进刹车时调用 Reset() 归零，恢复跟踪后同样 从零渐变。主控律共 4 个限幅器：yaw_deg_limiter_ / yaw_rate_limiter_ （度/秒²）、vx_limiter_ / vz_limiter_（米/秒²）。

3.5 像素→角度：全程针孔模型

PixelToAngle 构造期预计算针孔系数 2·tan(θ/2)/size，热路径仅一次 乘加 + atan：angle = atan(offset·2·tan(θ/2)/size)（规格 §4.1/§5.1）。 原"小偏移线性近似/大偏移针孔"双路切换已取消：线性式与针孔式在切换边界 两侧不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入 突变；atan 代价可忽略，全程采用精确针孔式。配置中 linear_ratio 键 随之移除。水平 β（PixelToAngle(W, θ_h)）与垂直 α（PixelToAngle(H, θ_v)） 各一个实例，构造期校验参数合法性（尺寸、视场角），非法抛 std::invalid_argument。

3.6 全工程仅 mavlink_command_gateway.cpp 一个 TU 包含 MAVLink 头

c_library_v2 的 mavlink_helpers.h 中 MAVLINK_HELPER 默认为空宏， 辅助函数直接以外部链接定义在头文件中；多个翻译单元包含 common/mavlink.h 会产生多重定义链接错误。因此：

  • common/mavlink.h 只被 src/mavlink_command_gateway.cpp 包含 （该文件也是全库唯一的 MAVLink 知识落点）；单元测试同样不得包含 该头，按 MAVLink v2 线格式手工解码字节流断言（见 §5.3）；
  • 公共头 mavlink_command_gateway.h 不出现任何 mavlink_* 类型， 打包/发送缓冲用成员固定字节存储承载（msg_storage_ / tx_buffer_， 各 512 字节，.cpp 中 static_assert 校验 ≥ sizeof(mavlink_message_t) 与 MAVLINK_MAX_PACKET_LEN）；
  • 打包用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入）， 不使用 _pack_chan 通道 API；
  • CMake 中 MAVLink include 目录设为 SYSTEM PRIVATE：压第三方告警噪音， 且不泄漏给下游。

同理，nlohmann/json 只被 src/vtc_config.cpp 一个 TU 包含。

3.7 配置启动期一次加载，不做热更新

TrackingConfig::LoadFromJson() 仅在启动期调用一次：解析结果拷贝进 TrackingConfig 值对象后即与 JSON 脱钩，热路径绝不触碰 JSON 对象； 调参重启即可。若未来需要热更新，扩展点为原子指针交换：持有 std::atomic<const TrackingConfig*>，后台解析新配置成功后原子换指针， 控制线程每拍读取当前指针快照使用（当前未实现，不引入额外复杂度）。

3.8 主控律单周期流程

Update() 按固定流程执行（last_now_ms_ 推算 dt，首拍与异常节拍 （时钟回拨/now_ms 未前进）用 1.0 / control.frequency_hz 兜底， dt 上限 kMaxDtS = 0.5 秒）：

① 超龄判定（姿态过期/缺失 → kDegradedHold；视觉超龄 → kDegradedHold） → ② 判丢刹车（清积分与限幅器历史）→ ③ Coast 清距离积分 → ④ 像素偏移 → β/α（针孔模型）→ ⑤ 水平航向（按 HeadingMode 分支） → ⑥ 垂直统一速度支路（α → vz_gain·α → 加速度限幅 → 速度限幅） → ⑦ 距离通道（distance_valid 且未超龄才可用；否则清积分并按 no_distance_action 降级）→ ⑧ 机体系→NED 旋转（RotateBodyToNed， 随 snapshot.yaw_rad）→ 置 valid = true 输出。

三个设计决策记录：

  • 垂直双支路合并（A1）：规格 §5.2 原设"小误差姿态微调（kAttitudeTrim → ID 82）/ 大误差垂直速度"双支路。姿态微调支路会把整个输出切成姿态 消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息类型 逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路：小误差经 vz_gain 线性映射为小垂直速度，行为连续、全周期单一 offboard 消息。
  • 位置式航向输出绝对值（A2）：ID 84 的 yaw 字段是绝对 NED 航向。若把 β 修正量直接填入，飞控会转向"修正角"方向而非目标方向。 位置式分支输出 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）

全库仅 ID 84 SET_POSITION_TARGET_LOCAL_NED 一种消息（原 kAttitudeTrim → ID 82 SET_ATTITUDE_TARGET 已随姿态微调支路移除）：

模式 消息 要点
kVelocityHeading ID 84 coordinate_frame = MAV_FRAME_LOCAL_NED（速度分量已是 NED 语义）。速度模式 type_mask 基础位 kMaskVelocityBase = 0x01C7（忽略位置与加速度），航向三分支：角速率非零 → kMaskVelocityYawRate（再忽略 yaw）；仅航向角非零 → kMaskVelocityYaw（再忽略 yaw_rate）；两者均零 → 速率掩码配零角速率（保持当前航向）——旧实现把零角速率落到航向掩码，等于误发绝对航向 0（指向正北）
kBrakeHover ID 84 kMaskBrake = 0x0DC7：速度全零，位置/加速度/航向/角速率字段全部忽略

四、日志行为

遵循 ../AGENTS.md 日志纪律：库内仅状态切换点打日志，热路径零日志、 零堆分配。

位置 日志 触发条件
tracking_control_law.cpp（Update() 内 FinishAndLog） spdlog::info "[ControlLaw] 状态切换: X -> Y (原因)" 仅当 ControlMode 相对 last_mode_ 发生变化时打印（刹车进出、降级切换、进入/退出 Coast 跟踪）；同一模式连续多拍不重复打印
tracking_control_law.cpp —— 每拍正常计算（含三路通道数学）不打任何日志
mavlink_command_gateway.cpp / pid_controller.cpp / line_of_sight.cpp / vtc_config.cpp —— 不打日志（配置错误走异常，见第六章）

示例程序（examples/track_control_demo.cpp，非生产代码）另有两处 演示观测日志，均带节流防刷屏：

  • LoggingTransport::Send()：字节统计与包头十六进制摘要，秒级节流 （距上次打印 ≥ 1000 ms 才输出）；
  • 主循环状态摘要：每 10 拍一次（20 Hz 下即半秒一次）。

五、构建、测试与运行方式

5.1 构建

../AGENTS.md 规定构建环境为 WSL2 Ubuntu 24.04。当前开发机 WSL 仅 装有 Ubuntu 22.04（发行版名 Ubuntu），本库的构建与测试验证在 22.04（cmake 3.22 / g++ 11.4）完成，行为与 24.04 无差异；24.04 发行版 安装后应按其复核一遍（见 §6.4）。

cmake -S visual_track_control -B build_vtc -DBUILD_TESTING=OFF
cmake --build build_vtc -j$(nproc)
./build_vtc/track_control_demo
  • 为什么 -DBUILD_TESTING=OFF：本库以 add_subdirectory 零改动复用 target_tracker，而 target_tracker/CMakeLists.txt 中 include(CTest) 会把 BUILD_TESTING 默认置为 ON，从而连带构建追踪库的 3 个测试 可执行文件。本子工程不关心其测试，显式关闭以加快构建（追踪库自身的 测试方式见 ../target_tracker/target_tracker.md §五）。
  • 本库自身测试不复用 BUILD_TESTING：由独立选项 VTC_BUILD_TESTS （默认 ON）控制，与上面的关闭互不干扰。
  • 示例无需 LD_LIBRARY_PATH：target_tracker 为 SHARED 库， 构建期 CMAKE_BUILD_RPATH 已指向构建目录，track_control_demo 可直接运行。
  • 示例缺省配置路径由编译期宏 VTC_DEMO_DEFAULT_CONFIG 内置为 config/track_control.json 的绝对路径；也可命令行传入 track_control_demo [配置文件路径]。

5.2 示例演示内容

单线程模拟完整链路（合成运动检测框 → TargetTracker::Update() → LastResult() + 打戳 → TrackingControlLaw::Update() → Gateway → LoggingTransport 字节打印），按配置频率固定节拍依次走三阶段 （对应规格 §6.4）：

  1. 正常跟踪（6 秒）：持续喂入向画面中心漂移的合成检测框，双目 距离有效，输出速率式速度航向意图（vx 按加速度限幅渐变至接近速度上限， 角速度随 β 收敛而减小）；
  2. Coast（4 秒）：停止喂检测 → is_predicted == true → 清积分、 继续预测跟踪；同时置距离无效，演示 §6.3 无距离降级路径（配置为 slow_approach：按限速值缓速前向）；
  3. 判丢刹车（3 秒）：连续丢帧超过追踪阈值 → tracked == false → 零速刹车。

示例将追踪库判丢阈值放大为 kDemoMaxLostFrames = 60 帧（默认 8 帧在 20 Hz 下 Coast 仅 0.4 秒不便观察），仅演示专用，正式参数以追踪库 规格为准。程序跑完总拍数后自行调用 TrackingControlLaw::Reset() 并 正常退出（返回 0），无需手动终止。

5.3 单元测试（Google Test）

测试源文件位于 tests/，覆盖 8 个测试套件：

测试文件 覆盖要点
rate_limiter_test.cpp Clamp 边界；SlewRateLimiter 单步限幅、dt<=0/冻结保护、Reset
staleness_checker_test.cpp 从未取样、时钟回拨（now < sample）、恰在有效期边界（临界判新鲜）
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 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道无距离降级动作（含 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、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播

运行方式（构建后）：

./build_vtc/vtc_unit_tests                      # 直接跑
ctest --test-dir build_vtc --output-on-failure  # 或经 ctest（gtest_discover_tests 注册）

依赖：系统安装 Google Test（libgtest-dev，FindGTest 模块模式，无需 CMake config 包）；未找到时自动跳过测试构建并打印状态，不影响库与示例。 vtc_config_test 通过编译期宏 VTC_TEST_CONFIG 读取真实配置文件 config/track_control.json——修改该配置文件时须同步核对测试断言。

5.4 部署提示

target_tracker 为 SHARED 库（libtarget_tracker.so.1）：部署 track_control_demo 或未来接入主工程时，必须将该 so 一并随带 （目录式部署与 RPATH 约定见 ../target_tracker/target_tracker.md §7）。 本库 visual_track_control 自身为 STATIC 库，链接进可执行文件，无需随带。

六、排查 / 修改要点

6.1 配置缺字段 / 非法值报错定位

LoadFromJson() 缺字段、类型错误、非法取值均抛 std::runtime_error， 异常消息携带完整字段名（如 配置缺失字段或类型错误: heading.gain、 配置非法(必须为正): control.frequency_hz、 配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit）， 按异常文本直接定位到 config/track_control.json 的对应分组与键即可； 文件打开/解析失败则携带文件路径。PixelToAngle 构造期参数非法（尺寸 非正、视场角不在 (0, π)）抛 std::invalid_argument，同样见消息文本。

6.2 MAVLink 消息待实测适配项

以下各项均以 PX4 1.11.3 Offboard 实测为准（规格 §10.1 待定项）， 全部集中在 src/mavlink_command_gateway.cpp，修改时只动这一个文件：

适配项 位置 现状
coordinate_frame SendVelocityHeading() / SendBrakeHover() 的 mavlink_msg_set_position_target_local_ned_pack 调用 固定取 MAV_FRAME_LOCAL_NED；速度分量已由主控律旋转为局部 NED 语义（§3.8 步骤⑧），语义是否匹配待实测确认
速度模式 type_mask 匿名命名空间 kMaskVelocityBase / kMaskVelocityYaw / kMaskVelocityYawRate / kMaskBrake 掩码位组合待实测确认

6.3 接入主工程时需补齐的事项

  1. 机体补偿（可选）：机体系→NED 旋转已在库内完成（仅绕下轴按 yaw_rad，规格 §2.3）；roll_rad / pitch_rad / yaw_rate_radps / velocity_* 字段仍为预留，规格 §4.1 的机体补偿启用时由姿态源填充；
  2. TrackedTarget 主题接线：TrackResult 尚未进入主工程 include/common/types.h；需先按 ../target_tracker/target_tracker.md §7 与规格 §7.2 正式定义 TrackedTarget 消息与 kTrackedTarget 主题， 由接线部件完成 TrackResult → TrackedTarget 转换与控制线程订阅；
  3. 传输实现替换：把示例的 LoggingTransport 换成真实链路实现 （ICommandTransport 注入），打包与控制律零改动；
  4. 距离有效标志来源：distance_valid 由双目链路（StereoRanger） 给出——距离来自目标检测框自身，控制律不自行判定；多目标场景下 调用方填 ControlSnapshot 时应校验 StereoTargetDistance.track_id 与 TrackResult.track_id 一致（v1.0 单目标为调用方约定，纪要 S3， 库内不加代码）。

6.4 红线声明

  • config/track_control.json 与 TrackingConfig 默认值中所有数值均为 占位值（对应规格第 8 章待定项与需求分析 §7 未标定前提）：相机内参/ 畸变/外参未标定、PID 与限幅参数未整定。标定与实测整定完成前，本库 仅限 SITL/台架录制数据回放验证数学链路，不得用于实飞。
  • 构建环境差异：../AGENTS.md 规定 WSL2 Ubuntu 24.04，当前开发机仅 装有 Ubuntu 22.04，验证在 22.04 完成；安装 24.04 后需复核构建与测试。
Reference in New Issue View Git Blame Copy Permalink