first commit
This commit is contained in:
@@ -0,0 +1,122 @@
|
||||
cmake_minimum_required(VERSION 3.16)
|
||||
|
||||
# =============================================================================
|
||||
# visual_track_control —— 视觉跟踪控制律静态库（独立子工程）
|
||||
#
|
||||
# 实现 docs/物理追踪思路.md 的水平航向控制（§4）、垂直/高度双支路控制（§5）、
|
||||
# 距离 PID（§6）、全量限幅（§8）、Coast 清积分与判丢刹车（§6.4）、
|
||||
# 超龄降级（§7.1、§9.2）。
|
||||
#
|
||||
# 构建约束：
|
||||
# - 与主工程完全解耦：根 CMakeLists.txt 零改动，可独立构建；
|
||||
# - 仅依赖 third_party：nlohmann（单头文件）、spdlog（源码）、
|
||||
# c_library_v2-master（MAVLink 纯头文件）；
|
||||
# - target_tracker 为 SHARED 库且零源码修改，原样 add_subdirectory 复用，
|
||||
# TrackResult 出现在本库公共头（vtc_types.h），故以 PUBLIC 链接；
|
||||
# - MAVLink include 目录设为 SYSTEM PRIVATE：压告警噪音且不泄漏给下游，
|
||||
# 全工程仅 src/mavlink_command_gateway.cpp 一个 TU 包含 mavlink.h
|
||||
# （MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义）。
|
||||
# =============================================================================
|
||||
|
||||
project(visual_track_control LANGUAGES CXX)
|
||||
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||
set(CMAKE_CXX_EXTENSIONS OFF)
|
||||
|
||||
# target_tracker 是 SHARED 库：BUILD_RPATH 指向构建目录，示例无需
|
||||
# LD_LIBRARY_PATH 即可直接运行（部署时需随带 libtarget_tracker.so.1）。
|
||||
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR};${CMAKE_BINARY_DIR}/target_tracker;$ORIGIN")
|
||||
|
||||
# ---- 依赖：target_tracker（零改动复用；其测试块由 -DBUILD_TESTING=OFF 压制） ----
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../target_tracker
|
||||
${CMAKE_BINARY_DIR}/target_tracker)
|
||||
|
||||
# ---- 依赖：spdlog（守卫引入，避免挂载进根构建时重复定义；关闭示例/测试/动态库/安装） ----
|
||||
if(NOT TARGET spdlog::spdlog)
|
||||
set(SPDLOG_BUILD_EXAMPLE OFF CACHE BOOL "" FORCE)
|
||||
set(SPDLOG_BUILD_EXAMPLE_HO OFF CACHE BOOL "" FORCE)
|
||||
set(SPDLOG_BUILD_TESTS OFF CACHE BOOL "" FORCE)
|
||||
set(SPDLOG_BUILD_TESTS_HO OFF CACHE BOOL "" FORCE)
|
||||
set(SPDLOG_BUILD_SHARED OFF CACHE BOOL "" FORCE)
|
||||
set(SPDLOG_INSTALL OFF CACHE BOOL "" FORCE)
|
||||
add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/../third_party/spdlog
|
||||
${CMAKE_BINARY_DIR}/spdlog)
|
||||
endif()
|
||||
|
||||
# ---- 静态库本体 ----
|
||||
add_library(visual_track_control STATIC
|
||||
src/vtc_config.cpp
|
||||
src/pid_controller.cpp
|
||||
src/line_of_sight.cpp
|
||||
src/tracking_control_law.cpp
|
||||
src/mavlink_command_gateway.cpp)
|
||||
|
||||
target_include_directories(visual_track_control PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>)
|
||||
|
||||
# nlohmann JSON：单头文件，仅配置加载器（启动期）使用，不进入热路径
|
||||
target_include_directories(visual_track_control PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../third_party/nlohmann)
|
||||
|
||||
# MAVLink 纯头文件库：SYSTEM 压告警、PRIVATE 不泄漏（公共头无任何 mavlink_* 类型）
|
||||
target_include_directories(visual_track_control SYSTEM PRIVATE
|
||||
${CMAKE_CURRENT_SOURCE_DIR}/../third_party/c_library_v2-master)
|
||||
|
||||
target_link_libraries(visual_track_control
|
||||
PUBLIC target_tracker
|
||||
PRIVATE spdlog::spdlog)
|
||||
target_compile_features(visual_track_control PUBLIC cxx_std_17)
|
||||
|
||||
# 编译告警：与主工程一致
|
||||
if(MSVC)
|
||||
target_compile_options(visual_track_control PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(visual_track_control PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ---- 示例可执行文件（合成检测 → 追踪 → 控制律 → MAVLink 字节流打印） ----
|
||||
add_executable(track_control_demo examples/track_control_demo.cpp)
|
||||
target_link_libraries(track_control_demo PRIVATE visual_track_control spdlog::spdlog)
|
||||
target_compile_definitions(track_control_demo PRIVATE
|
||||
VTC_DEMO_DEFAULT_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json")
|
||||
if(MSVC)
|
||||
target_compile_options(track_control_demo PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(track_control_demo PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# ---- 单元测试（Google Test） ----
|
||||
# 独立选项：构建本子工程时需 -DBUILD_TESTING=OFF 压制 target_tracker 的测试，
|
||||
# 故本库测试开关不复用 BUILD_TESTING，二者互不干扰。
|
||||
# 开发机未安装 GTest 时自动跳过（不影响库与示例构建）。
|
||||
option(VTC_BUILD_TESTS "构建 visual_track_control 单元测试" ON)
|
||||
if(VTC_BUILD_TESTS)
|
||||
find_package(GTest QUIET)
|
||||
if(GTest_FOUND)
|
||||
enable_testing()
|
||||
include(GoogleTest)
|
||||
add_executable(vtc_unit_tests
|
||||
tests/rate_limiter_test.cpp
|
||||
tests/staleness_checker_test.cpp
|
||||
tests/frame_transform_test.cpp
|
||||
tests/line_of_sight_test.cpp
|
||||
tests/pid_controller_test.cpp
|
||||
tests/tracking_control_law_test.cpp
|
||||
tests/vtc_config_test.cpp
|
||||
tests/mavlink_command_gateway_test.cpp)
|
||||
target_link_libraries(vtc_unit_tests PRIVATE
|
||||
visual_track_control GTest::gtest GTest::gtest_main)
|
||||
target_compile_definitions(vtc_unit_tests PRIVATE
|
||||
VTC_TEST_CONFIG="${CMAKE_CURRENT_SOURCE_DIR}/config/track_control.json"
|
||||
VTC_TEST_TMP_DIR="${CMAKE_CURRENT_BINARY_DIR}")
|
||||
if(MSVC)
|
||||
target_compile_options(vtc_unit_tests PRIVATE /W4 /permissive-)
|
||||
else()
|
||||
target_compile_options(vtc_unit_tests PRIVATE -Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
gtest_discover_tests(vtc_unit_tests)
|
||||
else()
|
||||
message(STATUS "未找到 GTest：跳过 vtc_unit_tests 单元测试")
|
||||
endif()
|
||||
endif()
|
||||
@@ -0,0 +1,48 @@
|
||||
{
|
||||
"_comment": "视觉跟踪控制律配置。全部数值为占位示例（对应 docs/物理追踪思路.md 第 8 章待定项），键名后缀标注单位；标定完成前仅限回放验证，不得用于实飞。",
|
||||
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"attitude_stale_ms": 300
|
||||
},
|
||||
|
||||
"camera": {
|
||||
"image_width": 1920,
|
||||
"image_height": 1080,
|
||||
"fov_h_deg": 60.0,
|
||||
"fov_v_deg": 35.0
|
||||
},
|
||||
|
||||
"heading": {
|
||||
"mode": "rate",
|
||||
"gain": 1.0,
|
||||
"yaw_rate_limit_dps": 30.0,
|
||||
"yaw_slew_limit_dps2": 60.0
|
||||
},
|
||||
|
||||
"vertical": {
|
||||
"vz_gain_mps_per_deg": 0.1,
|
||||
"vz_limit_mps": 2.0
|
||||
},
|
||||
|
||||
"distance": {
|
||||
"d_exp_m": 10.0,
|
||||
"kp": 0.5,
|
||||
"ki": 0.05,
|
||||
"kd": 0.1,
|
||||
"integral_limit": 2.0,
|
||||
"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
|
||||
},
|
||||
|
||||
"accel_limit": {
|
||||
"ax_mps2": 2.0,
|
||||
"ay_mps2": 2.0,
|
||||
"az_mps2": 1.5
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,225 @@
|
||||
// =============================================================================
|
||||
// track_control_demo.cpp —— 视觉跟踪控制律演示程序
|
||||
//
|
||||
// 数据流（模拟单线程整合的完整链路）：
|
||||
// 合成运动检测框 → TargetTracker::Update()（模拟推理线程每帧调用）
|
||||
// → LastResult() + 打戳 → ControlSnapshot → TrackingControlLaw::Update()
|
||||
// → MavlinkCommandGateway → LoggingTransport（字节数 + 十六进制摘要）
|
||||
//
|
||||
// 依次演示三种分支（对应物理追踪思路 §6.4）：
|
||||
// 1. 正常跟踪：持续喂入合成检测框，输出速度航向意图（垂直/距离通道
|
||||
// 统一为速度分量）；
|
||||
// 2. Coast：停止喂检测 → is_predicted==true → 清积分，继续预测跟踪；
|
||||
// 3. 判丢刹车：连续丢帧超过追踪库阈值 → tracked==false → 零速刹车。
|
||||
//
|
||||
// 运行方式：
|
||||
// track_control_demo [配置文件路径] # 缺省使用编译期内置路径
|
||||
// =============================================================================
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <cstdint>
|
||||
#include <cstdio>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
#include <target_tracker/target_tracker.h>
|
||||
|
||||
#include "visual_track_control/i_command_transport.h"
|
||||
#include "visual_track_control/mavlink_command_gateway.h"
|
||||
#include "visual_track_control/tracking_control_law.h"
|
||||
#include "visual_track_control/vtc_config.h"
|
||||
#include "visual_track_control/vtc_types.h"
|
||||
|
||||
#ifndef VTC_DEMO_DEFAULT_CONFIG
|
||||
#define VTC_DEMO_DEFAULT_CONFIG "config/track_control.json"
|
||||
#endif
|
||||
|
||||
namespace {
|
||||
|
||||
// ---- 演示时长（秒）：总时长有限，自动退出 ----
|
||||
constexpr int kNormalPhaseSeconds = 6; // 阶段1：正常跟踪
|
||||
constexpr int kCoastPhaseSeconds = 4; // 阶段2：Coast（停止喂检测）
|
||||
constexpr int kLostPhaseSeconds = 3; // 阶段3：判丢刹车
|
||||
|
||||
// 追踪库判丢阈值放大：默认 8 帧在 20Hz 下 Coast 仅 0.4 秒，不便于观察；
|
||||
// 演示专用放宽到 60 帧（约 3 秒），正式参数以追踪库规格为准。
|
||||
constexpr int kDemoMaxLostFrames = 60;
|
||||
|
||||
const char* ModeName(drone::vtc::ControlMode mode) {
|
||||
switch (mode) {
|
||||
case drone::vtc::ControlMode::kVelocityHeading: return "速度航向";
|
||||
case drone::vtc::ControlMode::kBrakeHover: return "零速刹车";
|
||||
case drone::vtc::ControlMode::kDegradedHold: return "降级保持";
|
||||
}
|
||||
return "未知";
|
||||
}
|
||||
|
||||
/// 演示用传输实现：统计字节数并打印十六进制摘要（按秒级节流，防止刷屏）
|
||||
class LoggingTransport : public drone::vtc::ICommandTransport {
|
||||
public:
|
||||
bool Send(const std::uint8_t* data, std::size_t len) override {
|
||||
++msg_count_;
|
||||
total_bytes_ += len;
|
||||
|
||||
const auto now_ms =
|
||||
std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now().time_since_epoch())
|
||||
.count();
|
||||
if (now_ms - last_print_ms_ >= 1000) {
|
||||
// 十六进制摘要：取包头前 12 字节
|
||||
std::string hex;
|
||||
char buf[4];
|
||||
const std::size_t n = len < 12 ? len : 12;
|
||||
for (std::size_t i = 0; i < n; ++i) {
|
||||
std::snprintf(buf, sizeof(buf), "%02X ", data[i]);
|
||||
hex += buf;
|
||||
}
|
||||
spdlog::info("[Transport] 累计 {} 帧 / {} 字节; 本帧 {} 字节, 包头: {}",
|
||||
msg_count_, total_bytes_, len, hex);
|
||||
last_print_ms_ = now_ms;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
std::int64_t last_print_ms_ = 0;
|
||||
std::uint64_t msg_count_ = 0;
|
||||
std::uint64_t total_bytes_ = 0;
|
||||
};
|
||||
|
||||
/// 合成运动检测框：从画面右下向中心缓慢漂移（模拟目标被持续锁定）
|
||||
std::vector<drone::tracker::Detection> SynthesizeDetections(double elapsed_s,
|
||||
int width, int height) {
|
||||
const double center_x = width * 0.5 + 160.0 - 20.0 * elapsed_s;
|
||||
const double center_y = height * 0.5 + 80.0 - 10.0 * elapsed_s;
|
||||
const double half_w = 70.0;
|
||||
const double half_h = 50.0;
|
||||
|
||||
drone::tracker::Detection det;
|
||||
det.x1 = center_x - half_w;
|
||||
det.y1 = center_y - half_h;
|
||||
det.x2 = center_x + half_w;
|
||||
det.y2 = center_y + half_h;
|
||||
det.conf = 0.9;
|
||||
det.cls_id = 0;
|
||||
return {det};
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// ---- 启动期：一次性加载配置（缺字段/非法值直接抛异常终止） ----
|
||||
const std::string config_path = argc > 1 ? argv[1] : VTC_DEMO_DEFAULT_CONFIG;
|
||||
drone::vtc::TrackingConfig cfg;
|
||||
try {
|
||||
cfg = drone::vtc::TrackingConfig::LoadFromJson(config_path);
|
||||
} catch (const std::exception& e) {
|
||||
spdlog::error("配置加载失败: {}", e.what());
|
||||
return 1;
|
||||
}
|
||||
spdlog::info("配置加载成功: {} (控制频率 {} Hz)", config_path,
|
||||
cfg.control.frequency_hz);
|
||||
|
||||
// ---- 构造：追踪库 / 控制律 / 传输 / 网关 ----
|
||||
drone::tracker::TargetTracker::Config tracker_cfg;
|
||||
tracker_cfg.max_lost_frames = kDemoMaxLostFrames; // 演示专用判丢阈值
|
||||
drone::tracker::TargetTracker tracker(tracker_cfg);
|
||||
|
||||
drone::vtc::TrackingControlLaw control_law(cfg);
|
||||
LoggingTransport transport;
|
||||
drone::vtc::MavlinkCommandGateway gateway(transport, /*sys=*/1, /*comp=*/1,
|
||||
/*target_sys=*/1, /*target_comp=*/1);
|
||||
|
||||
const int width = cfg.camera.image_width;
|
||||
const int height = cfg.camera.image_height;
|
||||
const int period_ms = static_cast<int>(1000.0 / cfg.control.frequency_hz);
|
||||
const int normal_ticks = kNormalPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
|
||||
const int coast_ticks = kCoastPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
|
||||
const int lost_ticks = kLostPhaseSeconds * static_cast<int>(cfg.control.frequency_hz);
|
||||
const int total_ticks = normal_ticks + coast_ticks + lost_ticks;
|
||||
|
||||
spdlog::info("演示开始: 正常跟踪 {}s → Coast {}s → 判丢刹车 {}s (共 {} 拍, {} ms/拍)",
|
||||
kNormalPhaseSeconds, kCoastPhaseSeconds, kLostPhaseSeconds,
|
||||
total_ticks, period_ms);
|
||||
|
||||
drone::tracker::FrameShape frame_shape;
|
||||
frame_shape.height = height;
|
||||
frame_shape.width = width;
|
||||
|
||||
const char* current_phase = nullptr;
|
||||
|
||||
// ---- 主循环：按配置频率固定节拍（§7.1） ----
|
||||
for (int tick = 0; tick < total_ticks; ++tick) {
|
||||
const double elapsed_s = static_cast<double>(tick) / cfg.control.frequency_hz;
|
||||
// 模拟单调时钟：1 秒偏移保证取样时间戳有效
|
||||
const std::int64_t now_ms = 1000 + static_cast<std::int64_t>(tick) * period_ms;
|
||||
|
||||
// ---- 阶段划分 ----
|
||||
const char* phase = tick < normal_ticks ? "正常跟踪"
|
||||
: tick < normal_ticks + coast_ticks ? "Coast"
|
||||
: "判丢刹车";
|
||||
if (phase != current_phase) {
|
||||
spdlog::info("===== 阶段切换: {} =====", phase);
|
||||
current_phase = phase;
|
||||
}
|
||||
|
||||
// ---- 模拟推理线程：仅正常跟踪阶段喂入检测框 ----
|
||||
// 阶段2/3 停止喂检测：先 Coast（is_predicted），后判丢（tracked==false）
|
||||
std::vector<drone::tracker::Detection> detections;
|
||||
if (tick < normal_ticks) {
|
||||
detections = SynthesizeDetections(elapsed_s, width, height);
|
||||
}
|
||||
tracker.Update(detections, frame_shape);
|
||||
|
||||
// ---- 控制线程：LastResult() + 打戳（TrackResult 无时间戳，§7.2） ----
|
||||
drone::vtc::ControlSnapshot snapshot;
|
||||
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;
|
||||
|
||||
// 自机姿态：演示中始终新鲜有效（接入主工程时来自 FlightStateSnapshot）
|
||||
snapshot.attitude_present = true;
|
||||
snapshot.attitude_time_ms = now_ms;
|
||||
|
||||
// ---- 控制律单周期计算 ----
|
||||
const drone::vtc::ControlOutput output = control_law.Update(snapshot, now_ms);
|
||||
|
||||
// ---- 按模式经网关发送 ----
|
||||
const auto time_boot_ms = static_cast<std::uint32_t>(now_ms);
|
||||
switch (output.mode) {
|
||||
case drone::vtc::ControlMode::kVelocityHeading:
|
||||
gateway.SendVelocityHeading(output, time_boot_ms);
|
||||
break;
|
||||
case drone::vtc::ControlMode::kBrakeHover:
|
||||
gateway.SendBrakeHover(time_boot_ms);
|
||||
break;
|
||||
case drone::vtc::ControlMode::kDegradedHold:
|
||||
break; // 降级保持：不发送指令（仅告警，§7.1）
|
||||
}
|
||||
|
||||
// ---- 半秒一次的摘要打印（演示观测用，非生产热路径） ----
|
||||
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 关联={}",
|
||||
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);
|
||||
}
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(period_ms));
|
||||
}
|
||||
|
||||
control_law.Reset(); // 停止视觉闭环：清全部状态（§6.4）
|
||||
spdlog::info("演示结束");
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// frame_transform.h —— 机体系（前/右/下）→ 局部 NED（北/东/地）坐标旋转
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §2.3 控制律内部计算统一在机体坐标系；输出的速度意图若采用局部系，
|
||||
// 必须随姿态完成旋转并显式标注坐标系，禁止混用未标注坐标系的数据。
|
||||
//
|
||||
// 实现要点：
|
||||
// - 速度设定值只随**航向角 ψ**（绕下轴旋转）旋转：水平速度分量的
|
||||
// 姿态耦合项由飞控内环处理，这是 PX4 offboard 速度设定值的标准做法；
|
||||
// 滚转/俯仰不参与（对速度指令为二阶小量）；
|
||||
// - 旋转为正交变换：合速度大小不变，因此"限幅在机体系完成、旋转在
|
||||
// 输出端执行"不破坏 §8 限幅约束；
|
||||
// - 无状态纯函数（零分配、无虚函数、无锁），可独立单元测试。
|
||||
//
|
||||
// 正方向验证（NED：x=北，y=东，z=地；航向 ψ 自北顺时针为正）：
|
||||
// - 机体前向 (1,0) 在 ψ=90°（机头朝东）时 → (0, 1) = 东 ✓
|
||||
// - 机体右向 (0,1) 在 ψ=0°（机头朝北）时 → (0, 1) = 东 ✓
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 机体系速度（前/右/下）→ 局部 NED（北/东/地），绕下轴按航向角 ψ 旋转：
|
||||
/// v_north = v_forward·cosψ − v_right·sinψ
|
||||
/// v_east = v_forward·sinψ + v_right·cosψ
|
||||
/// v_down = v_down（不变）
|
||||
/// 参数以引用传入并就地写出旋转结果。
|
||||
inline void RotateBodyToNed(double& v_forward, double& v_right, double& v_down,
|
||||
double yaw_rad) {
|
||||
(void)v_down; // 垂直分量不受绕下轴旋转影响；保留参数以保持接口对称
|
||||
const double c = std::cos(yaw_rad);
|
||||
const double s = std::sin(yaw_rad);
|
||||
const double north = v_forward * c - v_right * s;
|
||||
const double east = v_forward * s + v_right * c;
|
||||
v_forward = north;
|
||||
v_right = east;
|
||||
// v_down 不变
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,31 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// i_command_transport.h —— 命令传输层抽象接口
|
||||
//
|
||||
// 设计目标（双重解耦）：
|
||||
// - 传输层只吃字节流：串口 / 电台 / 回环打印均可注入实现，
|
||||
// 后续重写传输只换实现，不动打包与控制律；
|
||||
// - 打包封装（MavlinkCommandGateway）与本接口组合，不直接依赖具体链路。
|
||||
//
|
||||
// 控制律不直接写飞控（物理追踪思路 §2.2 控制单出口约束的独立库映射）：
|
||||
// 网关打包后交由实现方决定如何送达。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 命令传输接口：只吃字节流
|
||||
class ICommandTransport {
|
||||
public:
|
||||
virtual ~ICommandTransport() = default;
|
||||
|
||||
/// 发送一帧已打包的命令字节流。
|
||||
/// 返回 true = 已交付底层链路；false = 发送失败（调用方可计数/告警）。
|
||||
/// 实现方保证：不长期持有 data 指针（同步拷贝或同步写出）。
|
||||
virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// line_of_sight.h —— 像素偏移 → 视线角（针孔模型）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §4.1 水平视线角 β：
|
||||
// 针孔模型 β = atan( x_offset · 2·tan(θ_h/2) / W )
|
||||
// （线性近似 β ≈ x_offset · ( θ_h / W ) 仅在 |x_offset| 很小时成立）
|
||||
// §5.1 垂直视线角 α 同理（θ_v、H）
|
||||
//
|
||||
// 实现要点：
|
||||
// - 全程使用精确针孔模型：规格中的线性近似与针孔式在切换边界两侧
|
||||
// 函数值不连续（θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制
|
||||
// 环路注入突变；atan 计算代价可忽略，故直接全程采用精确式，
|
||||
// linear_ratio 配置项随之移除（§8 待定项收敛）；
|
||||
// - 构造期预计算针孔系数，热路径仅一次乘加 + atan
|
||||
// （零分配、无虚函数、无锁）。
|
||||
// =============================================================================
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 像素偏移 → 视线角（弧度）转换器；水平/垂直各构造一个实例
|
||||
class PixelToAngle {
|
||||
public:
|
||||
/// size_px：画面尺寸（W 或 H，像素，必须 > 0）；
|
||||
/// fov_rad：视场角（θ_h 或 θ_v，弧度，必须 ∈ (0, π)）。
|
||||
/// 非法参数抛 std::invalid_argument（构造期校验）。
|
||||
PixelToAngle(double size_px, double fov_rad);
|
||||
|
||||
/// 偏移（像素，水平右正 / 垂直下正）→ 视线角（弧度，同向为正）
|
||||
double Convert(double offset_px) const;
|
||||
|
||||
private:
|
||||
double pinhole_scale_; // 2·tan(θ/2) / size：针孔模型系数
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
+65
@@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// mavlink_command_gateway.h —— MAVLink 命令打包网关（对外声明）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md §2.2
|
||||
// - kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
|
||||
// - kBrakeHover → ID 84 全零速度
|
||||
// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
|
||||
//
|
||||
// 关键构建约束（务必遵守）：
|
||||
// - MAVLink 头（common/mavlink.h）只允许被本库的
|
||||
// src/mavlink_command_gateway.cpp 一个 TU 包含：
|
||||
// MAVLINK_HELPER 默认空宏，多 TU 包含会多重定义；
|
||||
// - 因此本头文件不出现任何 mavlink_* 类型，打包/发送缓冲用
|
||||
// 固定尺寸字节存储承载（尺寸在 .cpp 中 static_assert 校验）。
|
||||
//
|
||||
// 线程与资源：无线程、无 IO、公共方法非虚；成员固定缓冲，热路径零堆分配。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstddef>
|
||||
#include <cstdint>
|
||||
|
||||
#include "visual_track_control/i_command_transport.h"
|
||||
#include "visual_track_control/vtc_types.h"
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// MAVLink 命令打包网关：控制意图 → MAVLink 消息字节流 → 传输接口
|
||||
class MavlinkCommandGateway {
|
||||
public:
|
||||
/// transport：字节流传输实现（注入，生命周期由调用方保证）；
|
||||
/// system_id/component_id：本机 MAVLink 身份；
|
||||
/// target_system/target_component：飞控端身份。
|
||||
MavlinkCommandGateway(ICommandTransport& transport, std::uint8_t system_id,
|
||||
std::uint8_t component_id, std::uint8_t target_system,
|
||||
std::uint8_t target_component);
|
||||
|
||||
/// 速度与航向意图 → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式 type_mask）。
|
||||
/// 掩码选择：`yaw_rate_dps` 非零 → 角速率掩码；`yaw_deg` 非零 → 航向角掩码；
|
||||
/// 两者均为零 → 角速率掩码配零角速率（保持当前航向，避免误发绝对航向 0）。
|
||||
bool SendVelocityHeading(const ControlOutput& output, std::uint32_t time_boot_ms);
|
||||
|
||||
/// 零速刹车 → ID 84 全零速度（判丢等场景，§6.4）。
|
||||
bool SendBrakeHover(std::uint32_t time_boot_ms);
|
||||
|
||||
// 固定缓冲尺寸（.cpp 中与实际类型大小做 static_assert 校验）：
|
||||
static constexpr std::size_t kMsgBufferSize = 512; // ≥ sizeof(mavlink_message_t)
|
||||
static constexpr std::size_t kTxBufferSize = 512; // ≥ MAVLINK_MAX_PACKET_LEN
|
||||
|
||||
private:
|
||||
ICommandTransport& transport_; // 字节流传输实现（注入）
|
||||
|
||||
// MAVLink 身份
|
||||
std::uint8_t system_id_;
|
||||
std::uint8_t component_id_;
|
||||
std::uint8_t target_system_;
|
||||
std::uint8_t target_component_;
|
||||
|
||||
// 固定缓冲：热路径零堆分配（对齐 8 满足 mavlink_message_t 对齐要求）
|
||||
alignas(8) std::uint8_t msg_storage_[kMsgBufferSize]; // mavlink_message_t 承载
|
||||
std::uint8_t tx_buffer_[kTxBufferSize]; // 发送字节缓冲
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// pid_controller.h —— 位置式 PID 控制器（纯算法构件）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §6.2 距离控制律：前向速度指令 = PID(Δd)，Δd = d − d_exp
|
||||
// §8 PID 参数（Kp/Ki/Kd、积分限幅、微分滤波）全部待定/配置化
|
||||
//
|
||||
// 特性（热路径零分配、无虚函数、无锁）：
|
||||
// - 位置式输出：out = Kp·e + Ki·∫e + Kd·d(e)/dt；
|
||||
// - 积分钳位：积分项限幅在 ±integral_limit；
|
||||
// - anti-windup（条件积分）：输出饱和且本拍积分增量仍推深饱和时冻结积分，
|
||||
// 误差反号后允许积分回卷，防止积分饱和与残留；
|
||||
// - 微分一阶低通：derivative_filter_coef ∈ [0,1)，0 = 不滤波；
|
||||
// - ResetIntegral() / Reset() 均 O(1)（Coast 清积分，§6.4）。
|
||||
// =============================================================================
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// PID 参数（全部来自配置，代码不写死任何数值）
|
||||
struct PidParams {
|
||||
double kp = 0.0; // 比例增益
|
||||
double ki = 0.0; // 积分增益
|
||||
double kd = 0.0; // 微分增益
|
||||
double integral_limit = 0.0; // 积分项限幅（<=0 表示不限幅）
|
||||
double derivative_filter_coef = 0.0; // 微分一阶低通系数 ∈ [0,1)：越大滤波越强
|
||||
double output_limit = 0.0; // 输出限幅（<=0 表示不限幅）
|
||||
};
|
||||
|
||||
/// 位置式 PID 控制器
|
||||
class PidController {
|
||||
public:
|
||||
explicit PidController(const PidParams& params);
|
||||
|
||||
/// 单步更新：输入误差与采样间隔（秒），返回限幅后的输出。
|
||||
/// dt_s <= 0 时保持上次输出（异常节拍保护）。
|
||||
double Update(double error, double dt_s);
|
||||
|
||||
/// 清空积分项（§6.4：Coast 时防止积分饱和）；保留微分历史与上次输出
|
||||
void ResetIntegral();
|
||||
|
||||
/// 清空全部内部状态（积分、微分历史、上次输出）
|
||||
void Reset();
|
||||
|
||||
/// 上次输出（只读）
|
||||
double LastOutput() const { return last_output_; }
|
||||
|
||||
private:
|
||||
PidParams params_;
|
||||
|
||||
double integral_ = 0.0; // 积分累计（已钳位）
|
||||
double prev_error_ = 0.0; // 上次误差（微分用）
|
||||
double filtered_derivative_ = 0.0; // 低通后的微分值
|
||||
double last_output_ = 0.0; // 上次输出
|
||||
bool has_prev_error_ = false; // 是否存在有效微分历史
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// rate_limiter.h —— 限幅与变化率限制（纯算法构件，header-only）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §8 所有阈值集中配置：速度/加速度/方向变化率必须限幅（FR-033）
|
||||
// S6 首条视觉速度设定值必须经过限幅，避免位置→速度切换产生突变
|
||||
// （状态机设计.md S6）
|
||||
//
|
||||
// 特性（热路径零分配、无虚函数、无锁）：
|
||||
// - Clamp()：静态限幅；
|
||||
// - SlewRateLimiter：保存上次输出，按配置变化率限幅；初值置 0 时
|
||||
// 天然实现"首条指令突变抑制"（S6）。
|
||||
// =============================================================================
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 限幅：将 v 钳制在 [lo, hi]（调用方保证 lo <= hi）
|
||||
inline double Clamp(double v, double lo, double hi) {
|
||||
return std::min(std::max(v, lo), hi);
|
||||
}
|
||||
|
||||
/// 变化率限幅器：限制输出每秒最大变化量（单位由调用方决定，
|
||||
/// 如 度/秒^2 用于航向、米/秒^2 用于速度）
|
||||
class SlewRateLimiter {
|
||||
public:
|
||||
/// rate_limit：每秒最大变化量（<=0 表示冻结输出）；initial：初始输出值
|
||||
explicit SlewRateLimiter(double rate_limit, double initial = 0.0)
|
||||
: rate_limit_(rate_limit), output_(initial) {}
|
||||
|
||||
/// 朝 target 推进一个节拍：单步变化不超过 rate_limit·dt_s。
|
||||
/// dt_s <= 0 或 rate_limit <= 0 时保持当前输出（异常节拍保护）。
|
||||
double Limit(double target, double dt_s) {
|
||||
if (dt_s <= 0.0 || rate_limit_ <= 0.0) {
|
||||
return output_;
|
||||
}
|
||||
const double max_step = rate_limit_ * dt_s;
|
||||
output_ = Clamp(target, output_ - max_step, output_ + max_step);
|
||||
return output_;
|
||||
}
|
||||
|
||||
/// 复位输出值（刹车/重置时归零，保证恢复跟踪后指令从零渐变）
|
||||
void Reset(double value = 0.0) { output_ = value; }
|
||||
|
||||
/// 上次输出（只读）
|
||||
double LastOutput() const { return output_; }
|
||||
|
||||
private:
|
||||
double rate_limit_; // 每秒最大变化量
|
||||
double output_; // 当前输出
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// staleness_checker.h —— 输入超龄判定（单调毫秒时钟）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §7.1 超龄数据只能触发降级或告警，不能继续生成正常控制指令；
|
||||
// 超龄判定使用取样时间 + valid_for_ms（MessageHeader 语义）。
|
||||
//
|
||||
// 特性：无状态纯函数封装（零分配、无虚函数、无锁）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 输入超龄判定器（无状态，仅提供静态判定）
|
||||
class StalenessChecker {
|
||||
public:
|
||||
/// sample_time_ms：取样单调时钟毫秒（<=0 视为从未取样，直接判超龄）；
|
||||
/// now_ms：当前单调时钟毫秒；
|
||||
/// valid_for_ms：有效期（毫秒，配置给定）。
|
||||
/// 返回 true = 已超龄。
|
||||
static bool IsStale(std::int64_t sample_time_ms, std::int64_t now_ms,
|
||||
std::int64_t valid_for_ms) {
|
||||
if (sample_time_ms <= 0) {
|
||||
return true; // 从未取样：视为超龄（触发降级）
|
||||
}
|
||||
if (now_ms < sample_time_ms) {
|
||||
// 时间戳不一致（时钟源错配/打戳错误）：年龄不可信。
|
||||
// 单一单调时钟下不应发生；宁可降级也不信任不可核实的取样。
|
||||
return true;
|
||||
}
|
||||
return (now_ms - sample_time_ms) > valid_for_ms;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
+63
@@ -0,0 +1,63 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// tracking_control_law.h —— 视觉跟踪主控律（TrackingControlLaw）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §4 水平航向控制（增益 → slew → 角速度限幅，位置式/速率式按配置）
|
||||
// §5 垂直/高度控制（统一速度支路：α → 垂直速度，原双支路已合并）
|
||||
// §6 距离控制（PID(Δd) → 前向速度；未关联按配置降级，§6.3）
|
||||
// §6.4 Coast 清积分、判丢零速刹车
|
||||
// §7.1 超龄判定（姿态/视觉超龄 → 降级保持）
|
||||
// §8 统一限幅
|
||||
// §2.3 输出端随机体航向把机体系速度旋转为局部 NED
|
||||
//
|
||||
// 线程模型：单线程调用（状态机与控制线程），成员无锁、无堆对象；
|
||||
// 日志仅状态切换点（刹车进出、降级切换），高频热路径不打日志。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include "visual_track_control/line_of_sight.h"
|
||||
#include "visual_track_control/pid_controller.h"
|
||||
#include "visual_track_control/rate_limiter.h"
|
||||
#include "visual_track_control/vtc_config.h"
|
||||
#include "visual_track_control/vtc_types.h"
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 视觉跟踪主控律：消费控制快照，输出限幅后的控制意图
|
||||
class TrackingControlLaw {
|
||||
public:
|
||||
/// cfg：启动期加载的全量配置（值拷贝持有，运行期不再触碰配置源）
|
||||
explicit TrackingControlLaw(const TrackingConfig& cfg);
|
||||
|
||||
/// 单周期控制计算（§7.1 固定周期持续运行）。
|
||||
/// now_ms：当前单调时钟毫秒；内部据此推算 dt 并做超龄判定。
|
||||
/// 流程：①超龄判定 → ②判丢刹车 → ③Coast 清积分 → ④像素→视线角
|
||||
/// → ⑤水平航向 → ⑥垂直速度 → ⑦距离通道 → ⑧机体系→NED 旋转输出
|
||||
ControlOutput Update(const ControlSnapshot& snapshot, std::int64_t now_ms);
|
||||
|
||||
/// 停止视觉闭环时清全部状态（积分、限幅器历史、节拍时钟，§6.4）
|
||||
void Reset();
|
||||
|
||||
private:
|
||||
TrackingConfig cfg_; // 配置值拷贝（运行期只读）
|
||||
|
||||
// ---- 预计算构件（构造期完成，热路径零分配） ----
|
||||
PixelToAngle pixel_to_beta_; // 水平：x_offset → β（§4.1）
|
||||
PixelToAngle pixel_to_alpha_; // 垂直：y_offset → α（§5.1）
|
||||
PidController distance_pid_; // 距离通道 PID（§6.2）
|
||||
|
||||
// ---- 变化率限幅器（§8 / S6 首条指令限幅） ----
|
||||
SlewRateLimiter yaw_deg_limiter_; // 位置式航向变化率（度/秒^2）
|
||||
SlewRateLimiter yaw_rate_limiter_; // 速率式角速度变化率（度/秒^2）
|
||||
SlewRateLimiter vx_limiter_; // 前向加速度限幅（米/秒^2）
|
||||
SlewRateLimiter vz_limiter_; // 垂直加速度限幅（米/秒^2）
|
||||
|
||||
// ---- 节拍与状态（仅打日志用，不参与数学） ----
|
||||
std::int64_t last_now_ms_ = 0; // 上次计算时刻
|
||||
ControlMode last_mode_ = ControlMode::kDegradedHold; // 上次输出模式（状态切换日志）
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,113 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// vtc_config.h —— 视觉跟踪控制律配置（TrackingConfig + LoadFromJson）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §8 限幅、阈值与配置化（所有阈值集中配置，不得散落在代码中，
|
||||
// 需求分析.md NFR-007）
|
||||
// §4 水平航向控制（模式、增益、限幅）
|
||||
// §5 垂直/高度控制（垂直速度增益与限幅；原双支路已统一为速度支路）
|
||||
// §6 距离控制（射程、PID、限幅、无距离降级动作）
|
||||
// §7 控制周期与时序（频率、各输入超龄阈值）
|
||||
//
|
||||
// 设计约束：
|
||||
// - 全部数值待定：本结构默认值与示例配置均为占位，标定前仅限回放验证；
|
||||
// - LoadFromJson 启动期一次性解析：缺字段/类型错/非法值抛
|
||||
// std::runtime_error 并携带字段名（初始化抛异常规范）；
|
||||
// - 热路径绝不触碰 JSON 对象，不做热更新（调参重启即可）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
#include <string>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 航向通道模式（§4.2：位置式/速率式按 PX4 1.11.3 Offboard 实测选择）
|
||||
///
|
||||
/// 默认速率式：yaw_rate 指令天然是相对量，无坐标系歧义。位置式输出为
|
||||
/// 绝对 NED 航向（当前航向 + 限幅修正量合成），依赖新鲜自机姿态；
|
||||
/// 姿态接线未完成前不得用于实飞。
|
||||
enum class HeadingMode : std::uint8_t {
|
||||
kRate, // 速率式（默认）：β → 偏航角速度，输出 yaw_rate_dps
|
||||
kPosition, // 位置式：β → 修正量，与当前航向合成绝对航向输出 yaw_deg
|
||||
};
|
||||
|
||||
/// 雷达距离不可用（未关联/超龄）时的距离通道降级动作（§6.3，具体动作待定）
|
||||
enum class NoDistanceAction : std::uint8_t {
|
||||
kHold, // 保持：前向速度指令清零（安全占位语义）
|
||||
kSlowApproach, // 限速接近：按 no_distance_approach_limit_mps 缓速接近
|
||||
kExit, // 退出近距操作：输出降级保持（kDegradedHold）
|
||||
};
|
||||
|
||||
/// 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 attitude_stale_ms = 300; // 自机姿态超龄阈值（毫秒）
|
||||
};
|
||||
|
||||
/// camera 组：画面与视场角（§3.2、§4.1、§5.1；内参/畸变未标定，占位）
|
||||
struct CameraGroupConfig {
|
||||
int image_width = 1920; // 画面宽度 W（像素）
|
||||
int image_height = 1080; // 画面高度 H（像素）
|
||||
double fov_h_deg = 60.0; // 水平视场角 θ_h（度，仅水平，§4.1）
|
||||
double fov_v_deg = 35.0; // 垂直视场角 θ_v（度，§5.1）
|
||||
};
|
||||
|
||||
/// heading 组：水平航向通道（§4）
|
||||
struct HeadingGroupConfig {
|
||||
HeadingMode mode = HeadingMode::kRate; // 默认速率式（§4.2，见 HeadingMode 注释）
|
||||
double gain = 1.0; // 增益：视线角(度) → 航向指令（无量纲占位）
|
||||
double yaw_rate_limit_dps = 30.0; // 偏航角速度上限（度/秒，FR-033）
|
||||
double yaw_slew_limit_dps2 = 60.0; // 方向变化率上限（度/秒^2，FR-033、S6 首条限幅）
|
||||
};
|
||||
|
||||
/// vertical 组：垂直/高度控制（§5）
|
||||
///
|
||||
/// 规格 §5.2 原设"小误差姿态微调 / 大误差垂直速度"双支路，实测评审后
|
||||
/// 统一为速度支路（见 ControlMode 注释）：切换阈值与姿态微调参数随之移除。
|
||||
/// 小误差经 vz_gain 线性映射为小垂直速度，行为连续且无模式翻转。
|
||||
struct VerticalGroupConfig {
|
||||
double vz_gain_mps_per_deg = 0.1; // 垂直速度增益：α(度) → 垂直速度(米/秒)
|
||||
double vz_limit_mps = 2.0; // 垂直速度限幅（米/秒）
|
||||
};
|
||||
|
||||
/// distance 组：距离通道（§6）
|
||||
struct DistanceGroupConfig {
|
||||
double d_exp_m = 10.0; // 射程 d_exp（米，由拦截设备接口给出，§6.1 待定）
|
||||
double kp = 0.0; // PID 比例（§6.2 待定）
|
||||
double ki = 0.0; // PID 积分
|
||||
double kd = 0.0; // PID 微分
|
||||
double integral_limit = 0.0; // 积分项限幅
|
||||
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）
|
||||
double no_distance_approach_limit_mps = 0.5; // kSlowApproach 限速值（米/秒）
|
||||
};
|
||||
|
||||
/// accel_limit 组：各轴加速度限幅（§8，待定）
|
||||
struct AccelLimitConfig {
|
||||
double ax_mps2 = 2.0; // 前向加速度限幅（米/秒^2）
|
||||
double ay_mps2 = 2.0; // 侧向加速度限幅（米/秒^2，预留）
|
||||
double az_mps2 = 1.5; // 垂直加速度限幅（米/秒^2）
|
||||
};
|
||||
|
||||
/// 视觉跟踪控制律全量配置（§8：所有阈值集中于此）
|
||||
struct TrackingConfig {
|
||||
ControlGroupConfig control;
|
||||
CameraGroupConfig camera;
|
||||
HeadingGroupConfig heading;
|
||||
VerticalGroupConfig vertical;
|
||||
DistanceGroupConfig distance;
|
||||
AccelLimitConfig accel_limit;
|
||||
|
||||
/// 从 JSON 文件一次性加载配置（启动期调用）。
|
||||
/// 缺字段 / 类型错误 / 非法取值均抛 std::runtime_error，
|
||||
/// 异常消息携带字段名（如 "heading.gain"）。
|
||||
static TrackingConfig LoadFromJson(const std::string& path);
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,102 @@
|
||||
#pragma once
|
||||
|
||||
// =============================================================================
|
||||
// vtc_types.h —— 视觉跟踪控制律的中立输入/输出结构
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §2 输入输出与数据流（控制意图枚举语义）
|
||||
// §6.4 与追踪库的行为衔接（tracked / is_predicted 语义）
|
||||
// §7.2 与 target_tracker 的衔接（TrackResult 无时间戳，由调用方在
|
||||
// LastResult() 瞬间打戳）
|
||||
//
|
||||
// 设计约束：
|
||||
// - ControlSnapshot / ControlOutput 均为纯值类型（POD 风格），
|
||||
// 控制律热路径零堆分配；
|
||||
// - 本库不依赖主工程 include/ 的任何类型，独立可编译；
|
||||
// - 雷达距离必须带 associated_to_target 关联标志：未关联不得当作
|
||||
// 目标距离（§6.3、需求分析.md FR-041）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include <target_tracker/target_tracker.h>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
/// 控制输出模式（对应物理追踪思路 §2.2 的控制意图语义）
|
||||
///
|
||||
/// 注意：规格 §5.2 原设"垂直小误差姿态微调支路"（kAttitudeTrim →
|
||||
/// SET_ATTITUDE_TARGET），实测评审后取消——姿态微调支路会把整个输出切成
|
||||
/// 姿态消息，导致同周期的距离通道前向速度被丢弃（逼近停滞），且双消息
|
||||
/// 类型逐拍翻转会扰动 PX4 offboard。现垂直通道一律走速度支路（小误差
|
||||
/// 对应小 vz），全周期单一 offboard 消息。
|
||||
enum class ControlMode : std::uint8_t {
|
||||
kVelocityHeading, // 速度与航向意图：速度分量 + yaw（§4.2、§5、§6.2 主输出）
|
||||
kBrakeHover, // 零速刹车/悬停：判丢等场景（§6.4、§9.1）
|
||||
kDegradedHold, // 降级保持：超龄/退出近距操作，禁止正常控制（§7.1、§9.2）
|
||||
};
|
||||
|
||||
/// 降级原因（仅 kDegradedHold 模式有意义）
|
||||
enum class DegradedReason : std::uint8_t {
|
||||
kNone, // 未降级
|
||||
kAttitudeStale, // PX4 姿态过期：禁止空间转换和控制（§9.2）
|
||||
kVisionStale, // 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
|
||||
kNoDistanceExit, // 无距离关联且配置为退出近距操作（§6.3）
|
||||
};
|
||||
|
||||
/// 控制律输入快照：调用方在同一控制周期内采集并打戳
|
||||
///
|
||||
/// 注意：TrackResult 本身不含时间戳（§7.2），调用方必须在读取
|
||||
/// LastResult() 的瞬间用单调时钟打戳填入 track_time_ms；
|
||||
/// 超龄判定一律使用"取样时间 + valid_for_ms"（§7.1）。
|
||||
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; // 取样单调时钟（毫秒）
|
||||
|
||||
// ---- 自机姿态/速度 ----
|
||||
// 姿态过期 → 禁止控制（§9.2）。`yaw_rad` 参与输出端机体系→NED 旋转
|
||||
// （§2.3）与位置式绝对航向合成；其余字段预留，接入主工程时由
|
||||
// FlightStateSnapshot 填充（机体补偿见 §4.1）。
|
||||
bool attitude_present = false; // 姿态源当前是否可用
|
||||
std::int64_t attitude_time_ms = 0; // 姿态取样单调时钟（毫秒）
|
||||
double roll_rad = 0.0; // 横滚（弧度，FRD）
|
||||
double pitch_rad = 0.0; // 俯仰（弧度，FRD）
|
||||
double yaw_rad = 0.0; // 航向（弧度，NED）
|
||||
double yaw_rate_radps = 0.0; // 偏航角速度（弧度/秒，机体补偿预留，§4.1）
|
||||
double velocity_x_mps = 0.0; // 自机速度分量（预留）
|
||||
double velocity_y_mps = 0.0;
|
||||
double velocity_z_mps = 0.0;
|
||||
};
|
||||
|
||||
/// 控制律输出：限幅后的控制意图（§8：所有输出必须经过限幅）
|
||||
///
|
||||
/// 坐标语义：速度分量在机体系（前/右/下，§2.3）内计算与限幅，输出前
|
||||
/// 已随自机航向（ControlSnapshot.yaw_rad）旋转为局部 NED（北/东/地），
|
||||
/// 与网关使用的 MAV_FRAME_LOCAL_NED 一致；因此旋转后 vy_mps 可能非零
|
||||
/// （机体系侧向恒为 0，旋转不改变合速度大小，限幅在旋转前完成）。
|
||||
struct ControlOutput {
|
||||
ControlMode mode = ControlMode::kDegradedHold; // 输出模式
|
||||
bool valid = false; // 指令是否可用于发送
|
||||
|
||||
// ---- 速度分量（局部 NED：北/东/地，米/秒；由机体系随航向旋转得到） ----
|
||||
double vx_mps = 0.0; // 北向（距离通道输出经旋转，§6.2）
|
||||
double vy_mps = 0.0; // 东向（机体系恒 0，航向旋转后可非零）
|
||||
double vz_mps = 0.0; // 垂直（NED 下正，垂直速度支路输出，§5.2）
|
||||
|
||||
// ---- 航向通道（§4.2：位置式输出 yaw_deg，速率式输出 yaw_rate_dps） ----
|
||||
double yaw_deg = 0.0; // 目标绝对航向（度，NED，右转为正；仅位置式，
|
||||
// 由当前航向 + 限幅修正量合成并回绕 [-180,180)）
|
||||
double yaw_rate_dps = 0.0; // 偏航角速度指令（度/秒；速率式为默认，天然相对量）
|
||||
|
||||
// ---- 状态标注 ----
|
||||
DegradedReason degraded_reason = DegradedReason::kNone; // 降级原因
|
||||
bool coast_active = false; // 本周期处于 Coast（is_predicted==true，§6.4）
|
||||
};
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,42 @@
|
||||
// =============================================================================
|
||||
// line_of_sight.cpp —— 像素偏移 → 视线角实现（全程针孔模型）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md §4.1（水平 β）、§5.1（垂直 α）：
|
||||
// 针孔模型：angle = atan( offset · 2·tan(θ/2) / size )
|
||||
//
|
||||
// 原设计的线性近似双路切换已取消：线性式与针孔式在切换边界两侧不连续
|
||||
// （θ=60°、边界比例 0.2 处约 8% 角度台阶），会向控制环路注入突变；
|
||||
// atan 代价可忽略，全程采用精确针孔式。
|
||||
//
|
||||
// 构造期预计算针孔系数，Convert 热路径仅一次乘加 + atan。
|
||||
// =============================================================================
|
||||
|
||||
#include "visual_track_control/line_of_sight.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
namespace {
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
}
|
||||
|
||||
PixelToAngle::PixelToAngle(double size_px, double fov_rad) {
|
||||
// 构造期校验：非法参数直接抛异常（初始化抛异常规范）
|
||||
if (!(size_px > 0.0)) {
|
||||
throw std::invalid_argument("PixelToAngle: size_px 必须为正");
|
||||
}
|
||||
if (!(fov_rad > 0.0 && fov_rad < kPi)) {
|
||||
throw std::invalid_argument("PixelToAngle: fov_rad 必须在 (0, π) 内");
|
||||
}
|
||||
|
||||
// 预计算针孔系数（§4.1 公式）
|
||||
pinhole_scale_ = 2.0 * std::tan(fov_rad / 2.0) / size_px; // 2·tan(θ/2) / size
|
||||
}
|
||||
|
||||
double PixelToAngle::Convert(double offset_px) const {
|
||||
return std::atan(offset_px * pinhole_scale_); // 针孔模型，全偏移范围精确
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,119 @@
|
||||
// =============================================================================
|
||||
// mavlink_command_gateway.cpp —— MAVLink 命令打包网关实现
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md §2.2
|
||||
// kVelocityHeading → SET_POSITION_TARGET_LOCAL_NED（ID 84，速度模式）
|
||||
// kBrakeHover → ID 84 全零速度
|
||||
// （原 kAttitudeTrim → ID 82 已随姿态微调支路一并移除，见 vtc_types.h）
|
||||
//
|
||||
// 关键构建约束：本文件是全工程唯一包含 common/mavlink.h 的 TU。
|
||||
// MAVLINK_HELPER 默认空宏（mavlink_helpers.h），多 TU 包含会多重定义。
|
||||
//
|
||||
// 打包策略：
|
||||
// - 使用 mavlink_msg_*_pack（无通道状态、纯静态内联、可重入），
|
||||
// 不使用 _pack_chan 通道 API；
|
||||
// - mavlink_msg_to_send_buffer 输出字节流 → ICommandTransport::Send；
|
||||
// - 消息/发送缓冲为成员固定持有，热路径零堆分配。
|
||||
//
|
||||
// 坐标系与语义说明（待实测确认）：
|
||||
// - coordinate_frame 取 MAV_FRAME_LOCAL_NED；速度分量由主控律在输出端
|
||||
// 随机体航向旋转为局部 NED（北/东/地，物理追踪思路 §2.3），本文件
|
||||
// 直接填充不再做坐标变换；
|
||||
// - 速度模式 type_mask 需以 PX4 1.11.3 Offboard 实测为准
|
||||
// （物理追踪思路 §10.1）。
|
||||
// =============================================================================
|
||||
|
||||
#include "visual_track_control/mavlink_command_gateway.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "common/mavlink.h" // 全工程唯一包含点（见文件头构建约束）
|
||||
|
||||
namespace drone::vtc {
|
||||
namespace {
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kDegToRad = kPi / 180.0;
|
||||
|
||||
// ---- SET_POSITION_TARGET_LOCAL_NED（ID 84）type_mask 常量 ----
|
||||
// 掩码位语义：置位 = 忽略该字段。
|
||||
// 基础位：忽略位置 x/y/z（bit0-2）与加速度 afx/afy/afz（bit6-8）= 0x01C7。
|
||||
constexpr std::uint16_t kMaskVelocityBase = 0x01C7;
|
||||
// 速度 + 航向角模式：忽略 yaw_rate（bit11）
|
||||
constexpr std::uint16_t kMaskVelocityYaw = kMaskVelocityBase | 0x0800;
|
||||
// 速度 + 偏航角速率模式：忽略 yaw（bit10）
|
||||
constexpr std::uint16_t kMaskVelocityYawRate = kMaskVelocityBase | 0x0400;
|
||||
// 刹车模式：速度全零，yaw 与 yaw_rate 均忽略（bit10、bit11）
|
||||
constexpr std::uint16_t kMaskBrake = kMaskVelocityBase | 0x0C00;
|
||||
|
||||
/// 固定存储解释为消息结构（打包函数只写入字节，不读取旧值）
|
||||
inline mavlink_message_t* AsMessage(std::uint8_t* storage) {
|
||||
return reinterpret_cast<mavlink_message_t*>(storage);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
// 固定缓冲尺寸校验：编译期保证存储足够（头文件不暴露 mavlink 类型）
|
||||
static_assert(sizeof(mavlink_message_t) <= MavlinkCommandGateway::kMsgBufferSize,
|
||||
"msg_storage_ 不足以容纳 mavlink_message_t");
|
||||
static_assert(MAVLINK_MAX_PACKET_LEN <= MavlinkCommandGateway::kTxBufferSize,
|
||||
"tx_buffer_ 不足以容纳最大 MAVLink 数据包");
|
||||
|
||||
MavlinkCommandGateway::MavlinkCommandGateway(ICommandTransport& transport,
|
||||
std::uint8_t system_id,
|
||||
std::uint8_t component_id,
|
||||
std::uint8_t target_system,
|
||||
std::uint8_t target_component)
|
||||
: transport_(transport),
|
||||
system_id_(system_id),
|
||||
component_id_(component_id),
|
||||
target_system_(target_system),
|
||||
target_component_(target_component) {}
|
||||
|
||||
bool MavlinkCommandGateway::SendVelocityHeading(const ControlOutput& output,
|
||||
std::uint32_t time_boot_ms) {
|
||||
auto* msg = AsMessage(msg_storage_);
|
||||
|
||||
// 掩码三分支（A2 修复）：
|
||||
// 角速率非零 → 速率掩码（速率式控制律常态，§4.2）；
|
||||
// 航向角非零 → 航向掩码（位置式控制律）；
|
||||
// 两者均零 → 速率掩码配零角速率：保持当前航向。
|
||||
// 旧实现把"零角速率"落到航向掩码，等于误发绝对航向 0（指向正北）。
|
||||
const bool has_yaw_rate = std::fabs(output.yaw_rate_dps) > 1.0e-9;
|
||||
const bool has_yaw = std::fabs(output.yaw_deg) > 1.0e-9;
|
||||
const std::uint16_t type_mask =
|
||||
(!has_yaw_rate && has_yaw) ? kMaskVelocityYaw : kMaskVelocityYawRate;
|
||||
|
||||
mavlink_msg_set_position_target_local_ned_pack(
|
||||
system_id_, component_id_, msg,
|
||||
time_boot_ms, target_system_, target_component_,
|
||||
MAV_FRAME_LOCAL_NED, type_mask,
|
||||
0.0F, 0.0F, 0.0F, // 位置字段：忽略
|
||||
static_cast<float>(output.vx_mps), static_cast<float>(output.vy_mps),
|
||||
static_cast<float>(output.vz_mps),
|
||||
0.0F, 0.0F, 0.0F, // 加速度字段：忽略
|
||||
static_cast<float>(output.yaw_deg * kDegToRad),
|
||||
static_cast<float>(output.yaw_rate_dps * kDegToRad));
|
||||
|
||||
const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
|
||||
return transport_.Send(tx_buffer_, len);
|
||||
}
|
||||
|
||||
bool MavlinkCommandGateway::SendBrakeHover(std::uint32_t time_boot_ms) {
|
||||
auto* msg = AsMessage(msg_storage_);
|
||||
|
||||
// 判丢零速刹车（§6.4）：速度全零，位置/加速度/航向字段全部忽略
|
||||
mavlink_msg_set_position_target_local_ned_pack(
|
||||
system_id_, component_id_, msg,
|
||||
time_boot_ms, target_system_, target_component_,
|
||||
MAV_FRAME_LOCAL_NED, kMaskBrake,
|
||||
0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F, 0.0F,
|
||||
0.0F, 0.0F);
|
||||
|
||||
const std::uint16_t len = mavlink_msg_to_send_buffer(tx_buffer_, msg);
|
||||
return transport_.Send(tx_buffer_, len);
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,81 @@
|
||||
// =============================================================================
|
||||
// pid_controller.cpp —— 位置式 PID 控制器实现
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §6.2 前向速度指令 = PID(Δd)
|
||||
// §6.4 Coast 时积分项清空（ResetIntegral）
|
||||
//
|
||||
// anti-windup 策略（条件积分）：输出达到饱和限幅且本拍积分增量仍在
|
||||
// 推深饱和时，冻结本步积分累加（不采纳候选积分值），防止积分饱和；
|
||||
// 误差反号后增量反向、允许积分回卷，避免饱和残留。
|
||||
// =============================================================================
|
||||
|
||||
#include "visual_track_control/pid_controller.h"
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include "visual_track_control/rate_limiter.h"
|
||||
|
||||
namespace drone::vtc {
|
||||
|
||||
PidController::PidController(const PidParams& params) : params_(params) {}
|
||||
|
||||
double PidController::Update(double error, double dt_s) {
|
||||
if (dt_s <= 0.0) {
|
||||
return last_output_; // 异常节拍保护：保持上次输出
|
||||
}
|
||||
|
||||
// ---- 积分：候选值先钳位（积分限幅） ----
|
||||
double integral_candidate = integral_ + error * dt_s;
|
||||
if (params_.integral_limit > 0.0) {
|
||||
integral_candidate =
|
||||
Clamp(integral_candidate, -params_.integral_limit, params_.integral_limit);
|
||||
}
|
||||
|
||||
// ---- 微分：一阶低通滤波（系数 ∈ [0,1)，0 = 不滤波） ----
|
||||
double derivative = 0.0;
|
||||
if (has_prev_error_) {
|
||||
derivative = (error - prev_error_) / dt_s;
|
||||
const double coef = params_.derivative_filter_coef;
|
||||
derivative = coef * filtered_derivative_ + (1.0 - coef) * derivative;
|
||||
}
|
||||
filtered_derivative_ = derivative;
|
||||
prev_error_ = error;
|
||||
has_prev_error_ = true;
|
||||
|
||||
// ---- 位置式合成 ----
|
||||
const double integral_term = params_.ki * integral_candidate;
|
||||
const double raw = params_.kp * error + integral_term + params_.kd * derivative;
|
||||
|
||||
// ---- anti-windup（条件积分，C1 修复） ----
|
||||
// 冻结判据基于"本拍积分增量方向"（error·dt）：输出饱和且本拍增量仍
|
||||
// 在推深饱和时才冻结。不采用旧实现"积分项与输出同向"判据（误差反号
|
||||
// 后无法回卷），也不用 raw·error（大 kp 项会掩盖饱和方向、拖延回卷）。
|
||||
const bool saturated = params_.output_limit > 0.0 && std::fabs(raw) > params_.output_limit;
|
||||
const bool increment_pushes_deeper = (error * dt_s) * raw > 0.0;
|
||||
if (!(saturated && increment_pushes_deeper)) {
|
||||
integral_ = integral_candidate;
|
||||
}
|
||||
|
||||
// ---- 输出限幅 ----
|
||||
if (params_.output_limit > 0.0) {
|
||||
last_output_ = Clamp(raw, -params_.output_limit, params_.output_limit);
|
||||
} else {
|
||||
last_output_ = raw;
|
||||
}
|
||||
return last_output_;
|
||||
}
|
||||
|
||||
void PidController::ResetIntegral() {
|
||||
integral_ = 0.0; // §6.4：Coast 清积分，防积分饱和
|
||||
}
|
||||
|
||||
void PidController::Reset() {
|
||||
integral_ = 0.0;
|
||||
prev_error_ = 0.0;
|
||||
filtered_derivative_ = 0.0;
|
||||
last_output_ = 0.0;
|
||||
has_prev_error_ = false;
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,239 @@
|
||||
// =============================================================================
|
||||
// tracking_control_law.cpp —— 视觉跟踪主控律实现
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md
|
||||
// §4.1/§4.2 水平航向控制：像素偏移 → β → 增益 → slew → 角速度限幅，
|
||||
// 位置式输出 yaw_deg / 速率式输出 yaw_rate_dps（按配置）
|
||||
// §5.1/§5.2 垂直控制：统一速度支路 α → 垂直速度设定值（原"小误差姿态
|
||||
// 微调/大误差垂直速度"双支路已合并：姿态微调会把输出切成姿态
|
||||
// 消息，丢弃同周期距离通道前向速度导致逼近停滞，详见
|
||||
// vtc_types.h ControlMode 注释）
|
||||
// §6.2/§6.3 距离通道：Δd = d − d_exp → PID → 前向速度（限幅）；
|
||||
// 未关联/超龄 → 按配置降级动作（hold / slow_approach / exit）
|
||||
// §6.4 Coast（is_predicted）清航向/距离积分；判丢（!tracked）零速刹车
|
||||
// §7.1 超龄数据只触发降级：姿态过期 → 禁止控制；视觉过期 → 降级保持
|
||||
// §8 所有输出统一限幅；首条指令经变化率限幅（S6）
|
||||
// §2.3 输出端随机体航向把机体系速度旋转为局部 NED
|
||||
//
|
||||
// 日志纪律：仅状态切换点（刹车进出、降级切换）打印；热路径零日志、零分配。
|
||||
// =============================================================================
|
||||
|
||||
#include "visual_track_control/tracking_control_law.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
#include <spdlog/spdlog.h>
|
||||
|
||||
#include "visual_track_control/frame_transform.h"
|
||||
#include "visual_track_control/staleness_checker.h"
|
||||
|
||||
namespace drone::vtc {
|
||||
namespace {
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kRadToDeg = 180.0 / kPi;
|
||||
constexpr double kDegToRad = kPi / 180.0;
|
||||
constexpr double kMaxDtS = 0.5; // 单步 dt 上限（秒）：异常节拍保护
|
||||
|
||||
/// 角度回绕到 [-180, 180)（位置式绝对航向合成用）
|
||||
double WrapDeg180(double deg) {
|
||||
double d = std::fmod(deg + 180.0, 360.0);
|
||||
if (d < 0.0) {
|
||||
d += 360.0;
|
||||
}
|
||||
return d - 180.0;
|
||||
}
|
||||
|
||||
/// 控制模式可读名（仅状态切换日志使用）
|
||||
const char* ModeName(ControlMode mode) {
|
||||
switch (mode) {
|
||||
case ControlMode::kVelocityHeading: return "速度航向";
|
||||
case ControlMode::kBrakeHover: return "零速刹车";
|
||||
case ControlMode::kDegradedHold: return "降级保持";
|
||||
}
|
||||
return "未知";
|
||||
}
|
||||
|
||||
/// 由距离配置构造 PID 参数（§6.2）；输出限幅取接近/后退限幅的较大者
|
||||
PidParams BuildDistancePidParams(const TrackingConfig& cfg) {
|
||||
PidParams p;
|
||||
p.kp = cfg.distance.kp;
|
||||
p.ki = cfg.distance.ki;
|
||||
p.kd = cfg.distance.kd;
|
||||
p.integral_limit = cfg.distance.integral_limit;
|
||||
p.derivative_filter_coef = cfg.distance.derivative_filter_coef;
|
||||
p.output_limit = std::max(cfg.distance.approach_velocity_limit_mps,
|
||||
cfg.distance.retreat_velocity_limit_mps);
|
||||
return p;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TrackingControlLaw::TrackingControlLaw(const TrackingConfig& cfg)
|
||||
: cfg_(cfg),
|
||||
pixel_to_beta_(cfg.camera.image_width, cfg.camera.fov_h_deg * kDegToRad),
|
||||
pixel_to_alpha_(cfg.camera.image_height, cfg.camera.fov_v_deg * kDegToRad),
|
||||
distance_pid_(BuildDistancePidParams(cfg)),
|
||||
yaw_deg_limiter_(cfg.heading.yaw_slew_limit_dps2),
|
||||
yaw_rate_limiter_(cfg.heading.yaw_slew_limit_dps2),
|
||||
vx_limiter_(cfg.accel_limit.ax_mps2),
|
||||
vz_limiter_(cfg.accel_limit.az_mps2) {}
|
||||
|
||||
ControlOutput TrackingControlLaw::Update(const ControlSnapshot& snapshot,
|
||||
std::int64_t now_ms) {
|
||||
// ---- 节拍推算（§7.1 固定周期；异常节拍用配置频率兜底） ----
|
||||
// 时钟回拨或 now_ms 未前进（同值）时不信任差值，回退配置频率：
|
||||
// 单拍误差由限幅器与超龄判定兜底，不引入额外状态。
|
||||
double dt_s = 1.0 / cfg_.control.frequency_hz;
|
||||
if (last_now_ms_ > 0 && now_ms > last_now_ms_) {
|
||||
dt_s = std::min(static_cast<double>(now_ms - last_now_ms_) / 1000.0, kMaxDtS);
|
||||
}
|
||||
last_now_ms_ = now_ms;
|
||||
|
||||
ControlOutput out;
|
||||
out.mode = ControlMode::kVelocityHeading; // 正常路径默认模式，分支可覆盖
|
||||
|
||||
// 状态切换日志辅助：仅模式变化时打印（刹车进出、降级切换等）
|
||||
auto FinishAndLog = [&](const char* reason) {
|
||||
if (out.mode != last_mode_) {
|
||||
spdlog::info("[ControlLaw] 状态切换: {} -> {} ({})",
|
||||
ModeName(last_mode_), ModeName(out.mode), reason);
|
||||
last_mode_ = out.mode;
|
||||
}
|
||||
return out;
|
||||
};
|
||||
|
||||
// ---- ① 超龄判定（§7.1 / §9.2） ----
|
||||
// 姿态过期或缺失：禁止空间转换和控制，立即退出正常控制（§9.2）
|
||||
if (!snapshot.attitude_present ||
|
||||
StalenessChecker::IsStale(snapshot.attitude_time_ms, now_ms,
|
||||
cfg_.control.attitude_stale_ms)) {
|
||||
out.mode = ControlMode::kDegradedHold;
|
||||
out.degraded_reason = DegradedReason::kAttitudeStale;
|
||||
return FinishAndLog("姿态过期/缺失，禁止控制");
|
||||
}
|
||||
// 视觉追踪结果超龄：不得继续生成正常指令（§7.1）
|
||||
if (StalenessChecker::IsStale(snapshot.track_time_ms, now_ms,
|
||||
cfg_.control.visual_stale_ms)) {
|
||||
out.mode = ControlMode::kDegradedHold;
|
||||
out.degraded_reason = DegradedReason::kVisionStale;
|
||||
return FinishAndLog("视觉追踪结果超龄");
|
||||
}
|
||||
|
||||
// ---- ② 判丢 → 零速刹车（§6.4：tracked == false 执行 kBrakeHover） ----
|
||||
if (!snapshot.track.tracked) {
|
||||
if (last_mode_ != ControlMode::kBrakeHover) {
|
||||
// 进入刹车：清积分与限幅器历史，恢复跟踪后指令从零渐变（S6 首条限幅）
|
||||
distance_pid_.Reset();
|
||||
yaw_deg_limiter_.Reset();
|
||||
yaw_rate_limiter_.Reset();
|
||||
vx_limiter_.Reset();
|
||||
vz_limiter_.Reset();
|
||||
}
|
||||
out.mode = ControlMode::kBrakeHover; // 速度分量保持默认全零
|
||||
out.valid = true;
|
||||
return FinishAndLog("判丢，零速刹车");
|
||||
}
|
||||
|
||||
// ---- ③ Coast：is_predicted == true → 清积分防饱和（§6.4） ----
|
||||
if (snapshot.track.is_predicted) {
|
||||
distance_pid_.ResetIntegral();
|
||||
// 航向通道为纯比例增益映射（§4.2），无积分项，无需清理
|
||||
out.coast_active = true;
|
||||
}
|
||||
|
||||
// ---- ④ 像素偏移 → 视线角（§4.1 水平 β / §5.1 垂直 α） ----
|
||||
// x_offset = center.first − W/2（右正），y_offset = center.second − H/2（下正）
|
||||
const double x_offset =
|
||||
snapshot.track.center.first - cfg_.camera.image_width * 0.5;
|
||||
const double y_offset =
|
||||
snapshot.track.center.second - cfg_.camera.image_height * 0.5;
|
||||
const double beta_deg = pixel_to_beta_.Convert(x_offset) * kRadToDeg;
|
||||
const double alpha_deg = pixel_to_alpha_.Convert(y_offset) * kRadToDeg;
|
||||
|
||||
// ---- ⑤ 水平航向通道（§4.2：增益 → slew → 角速度限幅） ----
|
||||
if (cfg_.heading.mode == HeadingMode::kRate) {
|
||||
// 速率式：β → 偏航角速度指令，先变化率限幅再角速度限幅（FR-033）
|
||||
const double target_rate = cfg_.heading.gain * beta_deg;
|
||||
const double slewed = yaw_rate_limiter_.Limit(target_rate, dt_s);
|
||||
out.yaw_rate_dps = Clamp(slewed, -cfg_.heading.yaw_rate_limit_dps,
|
||||
cfg_.heading.yaw_rate_limit_dps);
|
||||
} else {
|
||||
// 位置式：β → 航向修正量，变化率限幅（方向变化率上限，FR-033）后
|
||||
// 与当前航向合成为绝对 NED 航向——ID 84 的 yaw 字段是绝对值，
|
||||
// 直接发修正量会让机体转向"修正角"方向（A2 修复）。姿态已在 ①
|
||||
// 通过超龄校验，yaw_rad 可用。
|
||||
const double target_yaw = cfg_.heading.gain * beta_deg;
|
||||
const double correction_deg = yaw_deg_limiter_.Limit(target_yaw, dt_s);
|
||||
out.yaw_deg = WrapDeg180(snapshot.yaw_rad * kRadToDeg + correction_deg);
|
||||
}
|
||||
|
||||
// ---- ⑥ 垂直速度通道（§5：统一速度支路） ----
|
||||
// 符号约定：α 下正（目标偏低）→ NED 下正 → vz 为正（下降趋近目标高度）。
|
||||
// 小误差自然映射为小垂直速度：无需单独的切换阈值与姿态微调支路，
|
||||
// 输出模式保持 kVelocityHeading，距离通道前向速度不会被丢弃（A1 修复）。
|
||||
{
|
||||
const double vz_target = cfg_.vertical.vz_gain_mps_per_deg * alpha_deg;
|
||||
const double slewed = vz_limiter_.Limit(vz_target, dt_s); // 加速度限幅（§8）
|
||||
out.vz_mps = Clamp(slewed, -cfg_.vertical.vz_limit_mps,
|
||||
cfg_.vertical.vz_limit_mps);
|
||||
}
|
||||
|
||||
// ---- ⑦ 距离通道（§6.2 / §6.3） ----
|
||||
// 雷达距离不得直接当目标距离：仅关联成立且未超龄才可用（§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);
|
||||
if (distance_available) {
|
||||
// Δ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 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,
|
||||
cfg_.distance.approach_velocity_limit_mps);
|
||||
} else {
|
||||
// 无可用目标距离：清积分防饱和，按配置降级动作（§6.3）
|
||||
distance_pid_.ResetIntegral();
|
||||
switch (cfg_.distance.no_distance_action) {
|
||||
case NoDistanceAction::kHold:
|
||||
// 保持：前向指令清零（安全占位语义；接入主工程后可改为保持当前速度）
|
||||
out.vx_mps = 0.0;
|
||||
vx_limiter_.Reset(0.0);
|
||||
break;
|
||||
case NoDistanceAction::kSlowApproach:
|
||||
// 限速接近：缓速前向，同样经加速度限幅
|
||||
out.vx_mps = vx_limiter_.Limit(
|
||||
cfg_.distance.no_distance_approach_limit_mps, dt_s);
|
||||
break;
|
||||
case NoDistanceAction::kExit:
|
||||
// 退出近距操作：整体降级保持
|
||||
out.mode = ControlMode::kDegradedHold;
|
||||
out.degraded_reason = DegradedReason::kNoDistanceExit;
|
||||
return FinishAndLog("距离未关联，退出近距操作");
|
||||
}
|
||||
}
|
||||
|
||||
// ---- ⑧ 机体系 → 局部 NED 旋转（§2.3，A4 修复） ----
|
||||
// 到此为止各通道限幅均在机体系完成（旋转为正交变换，合速度不变，
|
||||
// 限幅约束不被破坏）；输出端随快照航向旋转为北/东/地，与网关的
|
||||
// MAV_FRAME_LOCAL_NED 一致。姿态已在 ① 通过超龄校验，yaw_rad 可用。
|
||||
RotateBodyToNed(out.vx_mps, out.vy_mps, out.vz_mps, snapshot.yaw_rad);
|
||||
|
||||
out.valid = true;
|
||||
return FinishAndLog(out.coast_active ? "Coast 预测跟踪" : "正常跟踪");
|
||||
}
|
||||
|
||||
void TrackingControlLaw::Reset() {
|
||||
// 停止视觉闭环时清全部状态（§6.4）
|
||||
distance_pid_.Reset();
|
||||
yaw_deg_limiter_.Reset();
|
||||
yaw_rate_limiter_.Reset();
|
||||
vx_limiter_.Reset();
|
||||
vz_limiter_.Reset();
|
||||
last_now_ms_ = 0;
|
||||
last_mode_ = ControlMode::kDegradedHold;
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,225 @@
|
||||
// =============================================================================
|
||||
// vtc_config.cpp —— 配置加载器实现（启动期一次性解析）
|
||||
//
|
||||
// 对应规格：docs/物理追踪思路.md §8（阈值集中配置化，NFR-007）。
|
||||
//
|
||||
// 实现要点：
|
||||
// - 仅在启动期调用一次：缺字段 / 类型错误 / 非法取值抛
|
||||
// std::runtime_error，异常消息携带完整字段名（如 "heading.gain"）；
|
||||
// - 热路径绝不触碰 JSON：解析结果拷贝进 TrackingConfig 后即释放；
|
||||
// - 本文件是全库唯一包含 nlohmann/json 的 TU。
|
||||
// =============================================================================
|
||||
|
||||
#include "visual_track_control/vtc_config.h"
|
||||
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <nlohmann/json.hpp>
|
||||
|
||||
namespace drone::vtc {
|
||||
namespace {
|
||||
|
||||
using Json = nlohmann::json;
|
||||
|
||||
/// 读取并解析 JSON 文件；失败抛异常并携带路径
|
||||
Json ParseConfigFile(const std::string& path) {
|
||||
std::ifstream ifs(path);
|
||||
if (!ifs) {
|
||||
throw std::runtime_error("配置文件打开失败: " + path);
|
||||
}
|
||||
try {
|
||||
return Json::parse(ifs);
|
||||
} catch (const std::exception& e) {
|
||||
throw std::runtime_error("配置文件解析失败: " + path + " (" + e.what() + ")");
|
||||
}
|
||||
}
|
||||
|
||||
/// 取必需的对象分组；缺失或类型错抛异常
|
||||
const Json& RequireSection(const Json& root, const char* section) {
|
||||
const auto it = root.find(section);
|
||||
if (it == root.end() || !it->is_object()) {
|
||||
throw std::runtime_error(std::string("配置缺失分组或类型错误: ") + section);
|
||||
}
|
||||
return *it;
|
||||
}
|
||||
|
||||
/// 取必需的数值字段；缺失/类型错抛异常并携带字段名
|
||||
double RequireNumber(const Json& section, const char* section_name, const char* key) {
|
||||
const auto it = section.find(key);
|
||||
if (it == section.end() || !it->is_number()) {
|
||||
throw std::runtime_error(std::string("配置缺失字段或类型错误: ") +
|
||||
section_name + "." + key);
|
||||
}
|
||||
return it->get<double>();
|
||||
}
|
||||
|
||||
/// 取必需的整数字段
|
||||
std::int64_t RequireInteger(const Json& section, const char* section_name,
|
||||
const char* key) {
|
||||
const auto it = section.find(key);
|
||||
if (it == section.end() || !it->is_number_integer()) {
|
||||
throw std::runtime_error(std::string("配置缺失字段或类型错误(需整数): ") +
|
||||
section_name + "." + key);
|
||||
}
|
||||
return it->get<std::int64_t>();
|
||||
}
|
||||
|
||||
/// 取必需的字符串字段
|
||||
std::string RequireString(const Json& section, const char* section_name,
|
||||
const char* key) {
|
||||
const auto it = section.find(key);
|
||||
if (it == section.end() || !it->is_string()) {
|
||||
throw std::runtime_error(std::string("配置缺失字段或类型错误(需字符串): ") +
|
||||
section_name + "." + key);
|
||||
}
|
||||
return it->get<std::string>();
|
||||
}
|
||||
|
||||
/// 校验字段为正数；非法抛异常并携带字段名
|
||||
double RequirePositive(double value, const char* field_name) {
|
||||
if (!(value > 0.0)) {
|
||||
throw std::runtime_error(std::string("配置非法(必须为正): ") + field_name);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
/// 校验字段为非负数
|
||||
double RequireNonNegative(double value, const char* field_name) {
|
||||
if (value < 0.0) {
|
||||
throw std::runtime_error(std::string("配置非法(不得为负): ") + field_name);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
/// 校验字段在开区间 (lo, hi)
|
||||
double RequireRange(double value, double lo, double hi, const char* field_name) {
|
||||
if (!(value > lo && value < hi)) {
|
||||
throw std::runtime_error(std::string("配置非法(超出范围): ") + field_name);
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
ControlGroupConfig LoadControl(const Json& root) {
|
||||
const Json& s = RequireSection(root, "control");
|
||||
ControlGroupConfig c;
|
||||
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.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) {
|
||||
throw std::runtime_error("配置非法: control.*_stale_ms 必须为正");
|
||||
}
|
||||
return c;
|
||||
}
|
||||
|
||||
CameraGroupConfig LoadCamera(const Json& root) {
|
||||
const Json& s = RequireSection(root, "camera");
|
||||
CameraGroupConfig c;
|
||||
c.image_width = static_cast<int>(
|
||||
RequirePositive(RequireInteger(s, "camera", "image_width"), "camera.image_width"));
|
||||
c.image_height = static_cast<int>(
|
||||
RequirePositive(RequireInteger(s, "camera", "image_height"), "camera.image_height"));
|
||||
c.fov_h_deg = RequireRange(RequireNumber(s, "camera", "fov_h_deg"), 0.0, 180.0,
|
||||
"camera.fov_h_deg");
|
||||
c.fov_v_deg = RequireRange(RequireNumber(s, "camera", "fov_v_deg"), 0.0, 180.0,
|
||||
"camera.fov_v_deg");
|
||||
return c;
|
||||
}
|
||||
|
||||
HeadingGroupConfig LoadHeading(const Json& root) {
|
||||
const Json& s = RequireSection(root, "heading");
|
||||
HeadingGroupConfig h;
|
||||
const std::string mode = RequireString(s, "heading", "mode");
|
||||
if (mode == "position") {
|
||||
h.mode = HeadingMode::kPosition;
|
||||
} else if (mode == "rate") {
|
||||
h.mode = HeadingMode::kRate;
|
||||
} else {
|
||||
throw std::runtime_error("配置非法: heading.mode 仅支持 position / rate");
|
||||
}
|
||||
h.gain = RequireNonNegative(RequireNumber(s, "heading", "gain"), "heading.gain");
|
||||
h.yaw_rate_limit_dps = RequirePositive(
|
||||
RequireNumber(s, "heading", "yaw_rate_limit_dps"), "heading.yaw_rate_limit_dps");
|
||||
h.yaw_slew_limit_dps2 = RequirePositive(
|
||||
RequireNumber(s, "heading", "yaw_slew_limit_dps2"), "heading.yaw_slew_limit_dps2");
|
||||
return h;
|
||||
}
|
||||
|
||||
VerticalGroupConfig LoadVertical(const Json& root) {
|
||||
const Json& s = RequireSection(root, "vertical");
|
||||
VerticalGroupConfig v;
|
||||
v.vz_gain_mps_per_deg = RequireNonNegative(
|
||||
RequireNumber(s, "vertical", "vz_gain_mps_per_deg"), "vertical.vz_gain_mps_per_deg");
|
||||
v.vz_limit_mps = RequirePositive(RequireNumber(s, "vertical", "vz_limit_mps"),
|
||||
"vertical.vz_limit_mps");
|
||||
return v;
|
||||
}
|
||||
|
||||
DistanceGroupConfig LoadDistance(const Json& root) {
|
||||
const Json& s = RequireSection(root, "distance");
|
||||
DistanceGroupConfig d;
|
||||
d.d_exp_m = RequirePositive(RequireNumber(s, "distance", "d_exp_m"), "distance.d_exp_m");
|
||||
d.kp = RequireNonNegative(RequireNumber(s, "distance", "kp"), "distance.kp");
|
||||
d.ki = RequireNonNegative(RequireNumber(s, "distance", "ki"), "distance.ki");
|
||||
d.kd = RequireNonNegative(RequireNumber(s, "distance", "kd"), "distance.kd");
|
||||
d.integral_limit = RequireNonNegative(RequireNumber(s, "distance", "integral_limit"),
|
||||
"distance.integral_limit");
|
||||
d.derivative_filter_coef = RequireRange(
|
||||
RequireNumber(s, "distance", "derivative_filter_coef"), -0.000001, 1.0,
|
||||
"distance.derivative_filter_coef");
|
||||
if (d.derivative_filter_coef < 0.0) {
|
||||
d.derivative_filter_coef = 0.0; // 容许 -0.0
|
||||
}
|
||||
d.approach_velocity_limit_mps = RequirePositive(
|
||||
RequireNumber(s, "distance", "approach_velocity_limit_mps"),
|
||||
"distance.approach_velocity_limit_mps");
|
||||
d.retreat_velocity_limit_mps = RequirePositive(
|
||||
RequireNumber(s, "distance", "retreat_velocity_limit_mps"),
|
||||
"distance.retreat_velocity_limit_mps");
|
||||
const std::string action = RequireString(s, "distance", "no_distance_action");
|
||||
if (action == "hold") {
|
||||
d.no_distance_action = NoDistanceAction::kHold;
|
||||
} else if (action == "slow_approach") {
|
||||
d.no_distance_action = NoDistanceAction::kSlowApproach;
|
||||
} else if (action == "exit") {
|
||||
d.no_distance_action = NoDistanceAction::kExit;
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"配置非法: distance.no_distance_action 仅支持 hold / slow_approach / exit");
|
||||
}
|
||||
d.no_distance_approach_limit_mps = RequireNonNegative(
|
||||
RequireNumber(s, "distance", "no_distance_approach_limit_mps"),
|
||||
"distance.no_distance_approach_limit_mps");
|
||||
return d;
|
||||
}
|
||||
|
||||
AccelLimitConfig LoadAccelLimit(const Json& root) {
|
||||
const Json& s = RequireSection(root, "accel_limit");
|
||||
AccelLimitConfig a;
|
||||
a.ax_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ax_mps2"),
|
||||
"accel_limit.ax_mps2");
|
||||
a.ay_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "ay_mps2"),
|
||||
"accel_limit.ay_mps2");
|
||||
a.az_mps2 = RequirePositive(RequireNumber(s, "accel_limit", "az_mps2"),
|
||||
"accel_limit.az_mps2");
|
||||
return a;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TrackingConfig TrackingConfig::LoadFromJson(const std::string& path) {
|
||||
const Json root = ParseConfigFile(path);
|
||||
TrackingConfig cfg;
|
||||
cfg.control = LoadControl(root);
|
||||
cfg.camera = LoadCamera(root);
|
||||
cfg.heading = LoadHeading(root);
|
||||
cfg.vertical = LoadVertical(root);
|
||||
cfg.distance = LoadDistance(root);
|
||||
cfg.accel_limit = LoadAccelLimit(root);
|
||||
return cfg;
|
||||
}
|
||||
|
||||
} // namespace drone::vtc
|
||||
@@ -0,0 +1,56 @@
|
||||
// =============================================================================
|
||||
// frame_transform_test.cpp —— RotateBodyToNed 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §2.3，A4）：
|
||||
// 零航向恒等、朝东时机体前向 → 东向、垂直分量不变、模长守恒。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/frame_transform.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::RotateBodyToNed;
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
|
||||
TEST(FrameTransformTest, ZeroYawIsIdentity) {
|
||||
double x = 1.0, y = 0.5, z = -2.0;
|
||||
RotateBodyToNed(x, y, z, 0.0);
|
||||
EXPECT_NEAR(x, 1.0, 1e-12); // 北 = 前
|
||||
EXPECT_NEAR(y, 0.5, 1e-12); // 东 = 右
|
||||
EXPECT_NEAR(z, -2.0, 1e-12);
|
||||
}
|
||||
|
||||
TEST(FrameTransformTest, YawEast90MapsForwardToEast) {
|
||||
double x = 2.0, y = 0.0, z = 0.0;
|
||||
RotateBodyToNed(x, y, z, kPi / 2.0); // 机头朝东
|
||||
EXPECT_NEAR(x, 0.0, 1e-9); // 北向 ≈ 0
|
||||
EXPECT_NEAR(y, 2.0, 1e-9); // 东向 = 前向速度
|
||||
}
|
||||
|
||||
TEST(FrameTransformTest, RightIsEastWhenHeadingNorth) {
|
||||
double x = 0.0, y = 1.0, z = 0.0;
|
||||
RotateBodyToNed(x, y, z, 0.0);
|
||||
EXPECT_NEAR(x, 0.0, 1e-12);
|
||||
EXPECT_NEAR(y, 1.0, 1e-12);
|
||||
}
|
||||
|
||||
TEST(FrameTransformTest, DownComponentUnchanged) {
|
||||
double x = 1.0, y = 1.0, z = 3.5;
|
||||
RotateBodyToNed(x, y, z, kPi / 3.0);
|
||||
EXPECT_DOUBLE_EQ(z, 3.5);
|
||||
}
|
||||
|
||||
TEST(FrameTransformTest, PreservesHorizontalNorm) {
|
||||
// 旋转为正交变换：水平合速度不变（限幅约束不被破坏的前提）
|
||||
double x = 3.0, y = -4.0, z = 1.0;
|
||||
const double norm_before = std::hypot(x, y);
|
||||
RotateBodyToNed(x, y, z, 0.7);
|
||||
EXPECT_NEAR(std::hypot(x, y), norm_before, 1e-9);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,69 @@
|
||||
// =============================================================================
|
||||
// line_of_sight_test.cpp —— PixelToAngle 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §4.1 / §5.1，C2）：
|
||||
// 零偏移、画面边缘偏移 = 半视场角、奇对称、单调性、
|
||||
// 与线性近似的偏离（全程针孔模型）、构造期非法参数抛异常。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/line_of_sight.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::PixelToAngle;
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kRadToDeg = 180.0 / kPi;
|
||||
|
||||
TEST(PixelToAngleTest, ZeroOffsetGivesZeroAngle) {
|
||||
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
|
||||
EXPECT_DOUBLE_EQ(conv.Convert(0.0), 0.0);
|
||||
}
|
||||
|
||||
TEST(PixelToAngleTest, HalfSizeOffsetGivesHalfFov) {
|
||||
// 针孔模型：偏移 = size/2 时 angle = atan(tan(θ/2)) = θ/2
|
||||
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
|
||||
EXPECT_NEAR(conv.Convert(500.0), 45.0 * kPi / 180.0, 1e-12);
|
||||
EXPECT_NEAR(conv.Convert(-500.0), -45.0 * kPi / 180.0, 1e-12);
|
||||
}
|
||||
|
||||
TEST(PixelToAngleTest, OddSymmetry) {
|
||||
PixelToAngle conv(1920.0, 60.0 * kPi / 180.0);
|
||||
for (const double offset : {1.0, 100.0, 730.5}) {
|
||||
EXPECT_NEAR(conv.Convert(-offset), -conv.Convert(offset), 1e-15);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PixelToAngleTest, MonotonicIncreasing) {
|
||||
PixelToAngle conv(1080.0, 35.0 * kPi / 180.0);
|
||||
double prev = conv.Convert(-600.0);
|
||||
for (double x = -500.0; x <= 600.0; x += 100.0) {
|
||||
const double cur = conv.Convert(x);
|
||||
EXPECT_GT(cur, prev);
|
||||
prev = cur;
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PixelToAngleTest, DeviatesFromLinearApproximation) {
|
||||
// C2：全程针孔模型。θ=90°、偏移比例 0.25 处线性近似误差约 4°，
|
||||
// 针孔结果必须与之明显偏离（证明未走线性支路）。
|
||||
PixelToAngle conv(1000.0, 90.0 * kPi / 180.0);
|
||||
const double pinhole_deg = conv.Convert(250.0) * kRadToDeg; // atan(250·2·tan45°/1000)
|
||||
const double linear_deg = 250.0 * (90.0 / 1000.0); // θ·offset/size = 22.5°
|
||||
EXPECT_NEAR(pinhole_deg, std::atan(0.5) * kRadToDeg, 1e-9); // 26.565°
|
||||
EXPECT_GT(std::fabs(pinhole_deg - linear_deg), 2.0);
|
||||
}
|
||||
|
||||
TEST(PixelToAngleTest, ConstructorRejectsInvalidArgs) {
|
||||
EXPECT_THROW(PixelToAngle(0.0, 1.0), std::invalid_argument);
|
||||
EXPECT_THROW(PixelToAngle(-10.0, 1.0), std::invalid_argument);
|
||||
EXPECT_THROW(PixelToAngle(1000.0, 0.0), std::invalid_argument);
|
||||
EXPECT_THROW(PixelToAngle(1000.0, kPi), std::invalid_argument);
|
||||
EXPECT_THROW(PixelToAngle(1000.0, kPi + 0.1), std::invalid_argument);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,181 @@
|
||||
// =============================================================================
|
||||
// mavlink_command_gateway_test.cpp —— MavlinkCommandGateway 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §2.2、§4.2，A2 掩码三分支）：
|
||||
// ID 84 速度模式的 type_mask 选择（角速率非零 → 速率掩码；航向角非零 →
|
||||
// 航向掩码；两者均零 → 速率掩码配零角速率，保持航向）、
|
||||
// 速度/航向字段编码、刹车消息、坐标基准、传输失败传播。
|
||||
//
|
||||
// 构建约束：本文件**不得**包含 common/mavlink.h（MAVLINK_HELPER 默认空宏，
|
||||
// 多 TU 包含会多重定义），故按 MAVLink v2 线格式手工解码：
|
||||
// 帧头 10 字节（STX=0xFD, len, flags×2, seq, sys, comp, msgid×3 LE），
|
||||
// 载荷按字段尺寸降序排列（ID 84：time,x,y,z,vx,vy,vz,afx,afy,afz,
|
||||
// yaw,yaw_rate 均 4 字节在前，随后 u16 type_mask，再 target_system、
|
||||
// target_component、coordinate_frame 三个 u8）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <vector>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/i_command_transport.h"
|
||||
#include "visual_track_control/mavlink_command_gateway.h"
|
||||
#include "visual_track_control/vtc_types.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::ControlOutput;
|
||||
using drone::vtc::ICommandTransport;
|
||||
using drone::vtc::MavlinkCommandGateway;
|
||||
|
||||
// ---- MAVLink v2 帧布局常量（见文件头说明） ----
|
||||
constexpr std::uint8_t kMavlinkV2Stx = 0xFD;
|
||||
constexpr std::size_t kHeaderLen = 10;
|
||||
constexpr std::size_t kMsgId84PayloadLen = 53;
|
||||
|
||||
// ---- ID 84 载荷字段偏移（相对载荷起点） ----
|
||||
constexpr std::size_t kOffVx = 16;
|
||||
constexpr std::size_t kOffVy = 20;
|
||||
constexpr std::size_t kOffVz = 24;
|
||||
constexpr std::size_t kOffYaw = 40;
|
||||
constexpr std::size_t kOffYawRate = 44;
|
||||
constexpr std::size_t kOffTypeMask = 48;
|
||||
// u8 三字段线序（生成头 mavlink_msg_set_position_target_local_ned.h 为准）
|
||||
constexpr std::size_t kOffTargetSystem = 50;
|
||||
constexpr std::size_t kOffTargetComponent = 51;
|
||||
constexpr std::size_t kOffCoordFrame = 52;
|
||||
|
||||
// ---- 预期掩码值（与实现中常量一致：置位 = 忽略） ----
|
||||
// 基础 0x01C7（忽略位置与加速度）| 航向相关位
|
||||
constexpr std::uint16_t kExpectMaskYaw = 0x01C7 | 0x0800; // 忽略 yaw_rate
|
||||
constexpr std::uint16_t kExpectMaskYawRate = 0x01C7 | 0x0400; // 忽略 yaw
|
||||
constexpr std::uint16_t kExpectMaskBrake = 0x01C7 | 0x0C00; // 两者均忽略
|
||||
|
||||
/// 捕获式传输：记录最后一次发送的字节流，可注入失败
|
||||
class CaptureTransport : public ICommandTransport {
|
||||
public:
|
||||
bool Send(const std::uint8_t* data, std::size_t len) override {
|
||||
++send_count_;
|
||||
last_.assign(data, data + len);
|
||||
return succeed_;
|
||||
}
|
||||
|
||||
void SetSucceed(bool ok) { succeed_ = ok; }
|
||||
int send_count() const { return send_count_; }
|
||||
const std::vector<std::uint8_t>& last() const { return last_; }
|
||||
|
||||
private:
|
||||
bool succeed_ = true;
|
||||
int send_count_ = 0;
|
||||
std::vector<std::uint8_t> last_;
|
||||
};
|
||||
|
||||
float ReadFloat(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
|
||||
float v;
|
||||
std::memcpy(&v, buf.data() + kHeaderLen + payload_off, sizeof(v));
|
||||
return v;
|
||||
}
|
||||
|
||||
std::uint16_t ReadU16(const std::vector<std::uint8_t>& buf, std::size_t payload_off) {
|
||||
return static_cast<std::uint16_t>(buf[kHeaderLen + payload_off]) |
|
||||
(static_cast<std::uint16_t>(buf[kHeaderLen + payload_off + 1]) << 8);
|
||||
}
|
||||
|
||||
void ExpectId84Header(const std::vector<std::uint8_t>& buf) {
|
||||
ASSERT_GE(buf.size(), kHeaderLen + kMsgId84PayloadLen + 2); // +CRC
|
||||
EXPECT_EQ(buf[0], kMavlinkV2Stx);
|
||||
EXPECT_EQ(buf[1], kMsgId84PayloadLen);
|
||||
// msgid 24 位小端（字节 7-9）= 84
|
||||
EXPECT_EQ(buf[7], 84);
|
||||
EXPECT_EQ(buf[8], 0);
|
||||
EXPECT_EQ(buf[9], 0);
|
||||
}
|
||||
|
||||
constexpr double kDegToRad = 3.14159265358979323846 / 180.0;
|
||||
|
||||
TEST(MavlinkGatewayTest, YawRateModeSelectsRateMask) {
|
||||
CaptureTransport transport;
|
||||
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
|
||||
|
||||
ControlOutput out;
|
||||
out.mode = drone::vtc::ControlMode::kVelocityHeading;
|
||||
out.vx_mps = 1.5;
|
||||
out.vy_mps = -0.5;
|
||||
out.vz_mps = 0.25;
|
||||
out.yaw_rate_dps = 5.0;
|
||||
out.yaw_deg = 0.0;
|
||||
ASSERT_TRUE(gateway.SendVelocityHeading(out, 12345));
|
||||
|
||||
const auto& buf = transport.last();
|
||||
ExpectId84Header(buf);
|
||||
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 1.5F);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), -0.5F);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.25F);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), static_cast<float>(5.0 * kDegToRad));
|
||||
EXPECT_EQ(buf[kHeaderLen + kOffCoordFrame], 1); // MAV_FRAME_LOCAL_NED
|
||||
EXPECT_EQ(buf[kHeaderLen + kOffTargetSystem], 3); // target_system
|
||||
EXPECT_EQ(buf[kHeaderLen + kOffTargetComponent], 4); // target_component
|
||||
}
|
||||
|
||||
TEST(MavlinkGatewayTest, YawOnlySelectsYawMask) {
|
||||
CaptureTransport transport;
|
||||
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
|
||||
|
||||
ControlOutput out;
|
||||
out.mode = drone::vtc::ControlMode::kVelocityHeading;
|
||||
out.yaw_deg = 10.0;
|
||||
out.yaw_rate_dps = 0.0;
|
||||
ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
|
||||
|
||||
const auto& buf = transport.last();
|
||||
ExpectId84Header(buf);
|
||||
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYaw);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYaw), static_cast<float>(10.0 * kDegToRad));
|
||||
}
|
||||
|
||||
TEST(MavlinkGatewayTest, BothZeroKeepsHeadingViaRateMask) {
|
||||
// A2 回归：旧实现在零角速率时落到航向掩码，等于误发绝对航向 0（正北）。
|
||||
// 正确行为：速率掩码 + 零角速率 = 保持当前航向。
|
||||
CaptureTransport transport;
|
||||
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
|
||||
|
||||
ControlOutput out;
|
||||
out.mode = drone::vtc::ControlMode::kVelocityHeading;
|
||||
out.vx_mps = 1.0;
|
||||
ASSERT_TRUE(gateway.SendVelocityHeading(out, 1));
|
||||
|
||||
const auto& buf = transport.last();
|
||||
ExpectId84Header(buf);
|
||||
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskYawRate);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffYawRate), 0.0F);
|
||||
}
|
||||
|
||||
TEST(MavlinkGatewayTest, BrakeHoverSendsZeroVelocityAllIgnored) {
|
||||
CaptureTransport transport;
|
||||
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
|
||||
ASSERT_TRUE(gateway.SendBrakeHover(777));
|
||||
|
||||
const auto& buf = transport.last();
|
||||
ExpectId84Header(buf);
|
||||
EXPECT_EQ(ReadU16(buf, kOffTypeMask), kExpectMaskBrake);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVx), 0.0F);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVy), 0.0F);
|
||||
EXPECT_FLOAT_EQ(ReadFloat(buf, kOffVz), 0.0F);
|
||||
}
|
||||
|
||||
TEST(MavlinkGatewayTest, TransportFailurePropagates) {
|
||||
CaptureTransport transport;
|
||||
transport.SetSucceed(false);
|
||||
MavlinkCommandGateway gateway(transport, 1, 2, 3, 4);
|
||||
|
||||
ControlOutput out;
|
||||
out.mode = drone::vtc::ControlMode::kVelocityHeading;
|
||||
EXPECT_FALSE(gateway.SendVelocityHeading(out, 1));
|
||||
EXPECT_FALSE(gateway.SendBrakeHover(1));
|
||||
EXPECT_EQ(transport.send_count(), 2);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,123 @@
|
||||
// =============================================================================
|
||||
// pid_controller_test.cpp —— PidController 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §6.2 / §6.4）：
|
||||
// 位置式 P/I/D 三项、积分钳位、输出限幅、异常节拍保护、
|
||||
// anti-windup 条件积分（C1 修复回归：饱和冻结 + 误差反号回卷）、
|
||||
// ResetIntegral / Reset 行为。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/pid_controller.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::PidController;
|
||||
using drone::vtc::PidParams;
|
||||
|
||||
PidParams MakeParams(double kp, double ki, double kd, double integral_limit,
|
||||
double output_limit, double filter_coef = 0.0) {
|
||||
PidParams p;
|
||||
p.kp = kp;
|
||||
p.ki = ki;
|
||||
p.kd = kd;
|
||||
p.integral_limit = integral_limit;
|
||||
p.derivative_filter_coef = filter_coef;
|
||||
p.output_limit = output_limit;
|
||||
return p;
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, ProportionalOnly) {
|
||||
PidController pid(MakeParams(2.0, 0.0, 0.0, 0.0, 0.0));
|
||||
EXPECT_DOUBLE_EQ(pid.Update(3.0, 0.1), 6.0);
|
||||
EXPECT_DOUBLE_EQ(pid.Update(-1.5, 0.1), -3.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, IntegralAccumulates) {
|
||||
PidController pid(MakeParams(0.0, 1.0, 0.0, 0.0, 0.0));
|
||||
// 两拍 error=2、dt=0.5：积分累计 1.0 → 2.0
|
||||
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 1.0);
|
||||
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.5), 2.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, IntegralClampedToLimit) {
|
||||
PidController pid(MakeParams(0.0, 1.0, 0.0, /*integral_limit=*/1.0, 0.0));
|
||||
for (int i = 0; i < 5; ++i) {
|
||||
pid.Update(10.0, 1.0);
|
||||
}
|
||||
// 积分候选值被钳位在 1.0 → 输出恒为 1.0
|
||||
EXPECT_DOUBLE_EQ(pid.LastOutput(), 1.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, OutputClampedToLimit) {
|
||||
PidController pid(MakeParams(10.0, 0.0, 0.0, 0.0, /*output_limit=*/3.0));
|
||||
EXPECT_DOUBLE_EQ(pid.Update(1.0, 0.1), 3.0);
|
||||
EXPECT_DOUBLE_EQ(pid.Update(-1.0, 0.1), -3.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, AntiWindupFreezeWhileSaturatedSameDirection) {
|
||||
// kp=0、ki=2、output_limit=5：error=+1 持续饱和时积分冻结，
|
||||
// 输出恒钉在 +5（10 拍累计 10 拍也不会让积分继续增长）。
|
||||
PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
|
||||
for (int i = 0; i < 10; ++i) {
|
||||
EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, AntiWindupUnwindsAfterErrorFlips) {
|
||||
// C1 回归：误差反号后积分必须立即回卷（旧判据"积分与输出同向"会
|
||||
// 冻结回卷、输出一直钉在 +5）。
|
||||
// kp=0、ki=2、limit=5：+10 饱和 3 拍、积分冻结在 0；
|
||||
// error=-1 首拍 → 输出 2×(-1) = -2（立即回卷）；
|
||||
// 持续负误差使输出到达负饱和 -5（积分冻结在 -2，输出 = clamp(-6)）。
|
||||
PidController pid(MakeParams(0.0, 2.0, 0.0, 0.0, 5.0));
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
EXPECT_DOUBLE_EQ(pid.Update(10.0, 1.0), 5.0);
|
||||
}
|
||||
EXPECT_DOUBLE_EQ(pid.Update(-1.0, 1.0), -2.0); // 首拍立即回卷
|
||||
double out = -2.0;
|
||||
for (int i = 1; i < 12; ++i) {
|
||||
out = pid.Update(-1.0, 1.0);
|
||||
}
|
||||
EXPECT_NEAR(out, -5.0, 1e-9); // 旧判据此处仍为 +5.0
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, DerivativeFirstStepIsZero) {
|
||||
PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0));
|
||||
// 首拍无微分历史：微分项为 0
|
||||
EXPECT_DOUBLE_EQ(pid.Update(5.0, 1.0), 0.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, DerivativeWithLowPassFilter) {
|
||||
PidController pid(MakeParams(0.0, 0.0, 1.0, 0.0, 0.0, /*filter=*/0.5));
|
||||
pid.Update(0.0, 1.0); // 建立微分历史（导数 0）
|
||||
// 第二拍原始导数 1.0，低通后 0.5×0 + 0.5×1.0 = 0.5
|
||||
EXPECT_DOUBLE_EQ(pid.Update(1.0, 1.0), 0.5);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, ZeroOrNegativeDtHoldsLastOutput) {
|
||||
PidController pid(MakeParams(1.0, 0.0, 0.0, 0.0, 0.0));
|
||||
EXPECT_DOUBLE_EQ(pid.Update(2.0, 0.1), 2.0);
|
||||
EXPECT_DOUBLE_EQ(pid.Update(99.0, 0.0), 2.0);
|
||||
EXPECT_DOUBLE_EQ(pid.Update(99.0, -1.0), 2.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, ResetIntegralClearsOnlyIntegral) {
|
||||
PidController pid(MakeParams(0.0, 1.0, 1.0, 0.0, 0.0));
|
||||
pid.Update(2.0, 1.0); // 积分 = 2
|
||||
pid.ResetIntegral();
|
||||
// 积分清零、微分历史保留：输出应为 kd×(0−2)/1 = −2
|
||||
EXPECT_DOUBLE_EQ(pid.Update(0.0, 1.0), -2.0);
|
||||
}
|
||||
|
||||
TEST(PidControllerTest, ResetClearsEverything) {
|
||||
PidController pid(MakeParams(1.0, 1.0, 1.0, 0.0, 0.0));
|
||||
pid.Update(5.0, 1.0);
|
||||
pid.Reset();
|
||||
EXPECT_DOUBLE_EQ(pid.LastOutput(), 0.0);
|
||||
// 全清后等同全新控制器：首拍无微分历史
|
||||
EXPECT_DOUBLE_EQ(pid.Update(3.0, 1.0), 3.0 + 3.0); // kp·e + ki·∫e
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,76 @@
|
||||
// =============================================================================
|
||||
// rate_limiter_test.cpp —— Clamp / SlewRateLimiter 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §8、状态机设计 S6）：
|
||||
// 静态限幅、变化率限幅步进、首条指令从零渐变（S6）、
|
||||
// 异常节拍保护、Reset 归零。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/rate_limiter.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::Clamp;
|
||||
using drone::vtc::SlewRateLimiter;
|
||||
|
||||
TEST(ClampTest, ClampsValue) {
|
||||
EXPECT_DOUBLE_EQ(Clamp(5.0, -2.0, 2.0), 2.0);
|
||||
EXPECT_DOUBLE_EQ(Clamp(-5.0, -2.0, 2.0), -2.0);
|
||||
EXPECT_DOUBLE_EQ(Clamp(1.5, -2.0, 2.0), 1.5);
|
||||
// 边界值：落在区间内
|
||||
EXPECT_DOUBLE_EQ(Clamp(2.0, -2.0, 2.0), 2.0);
|
||||
EXPECT_DOUBLE_EQ(Clamp(-2.0, -2.0, 2.0), -2.0);
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, FirstCommandRampsFromZero) {
|
||||
// S6：初始输出为 0，首条大指令只能按变化率渐变
|
||||
SlewRateLimiter lim(/*rate_limit=*/2.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 1.0); // 0 + 2×0.5
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.5), 2.0); // 继续渐变
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 10.0), 10.0); // 步长足够 → 到达目标
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, TracksTargetWithinStep) {
|
||||
SlewRateLimiter lim(10.0);
|
||||
// 步长 10×1=10，目标 3 在步长内 → 直接到达
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(3.0, 1.0), 3.0); // 已在目标：保持
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, NegativeDirection) {
|
||||
SlewRateLimiter lim(2.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -1.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(-10.0, 0.5), -2.0);
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, ZeroOrNegativeDtHolds) {
|
||||
SlewRateLimiter lim(2.0);
|
||||
lim.Limit(10.0, 1.0); // 输出 2.0
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 0.0), 2.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, -1.0), 2.0);
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, ZeroRateLimitFreezesOutput) {
|
||||
SlewRateLimiter lim(0.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(5.0, 1.0), 0.0);
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, ResetToValue) {
|
||||
SlewRateLimiter lim(2.0);
|
||||
lim.Limit(10.0, 1.0);
|
||||
lim.Reset();
|
||||
EXPECT_DOUBLE_EQ(lim.LastOutput(), 0.0);
|
||||
lim.Reset(3.5);
|
||||
EXPECT_DOUBLE_EQ(lim.LastOutput(), 3.5);
|
||||
// 复位后从新值继续渐变
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 5.5); // 3.5 + 2×1
|
||||
}
|
||||
|
||||
TEST(SlewRateLimiterTest, CustomInitialValue) {
|
||||
SlewRateLimiter lim(1.0, /*initial=*/5.0);
|
||||
EXPECT_DOUBLE_EQ(lim.Limit(10.0, 1.0), 6.0);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,41 @@
|
||||
// =============================================================================
|
||||
// staleness_checker_test.cpp —— StalenessChecker 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §7.1）：
|
||||
// 从未取样判超龄、有效期内新鲜、超出有效期超龄、
|
||||
// 边界恰好在有效期上（不算超龄）、时间戳不一致判超龄（C4）。
|
||||
// =============================================================================
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/staleness_checker.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::StalenessChecker;
|
||||
|
||||
TEST(StalenessCheckerTest, NeverSampledIsStale) {
|
||||
EXPECT_TRUE(StalenessChecker::IsStale(0, 1000, 200));
|
||||
EXPECT_TRUE(StalenessChecker::IsStale(-5, 1000, 200));
|
||||
}
|
||||
|
||||
TEST(StalenessCheckerTest, WithinValidityIsFresh) {
|
||||
EXPECT_FALSE(StalenessChecker::IsStale(900, 1000, 200));
|
||||
EXPECT_FALSE(StalenessChecker::IsStale(1000, 1000, 200)); // 刚取样
|
||||
}
|
||||
|
||||
TEST(StalenessCheckerTest, BeyondValidityIsStale) {
|
||||
EXPECT_TRUE(StalenessChecker::IsStale(799, 1000, 200)); // 201 > 200
|
||||
}
|
||||
|
||||
TEST(StalenessCheckerTest, ExactlyAtBoundaryIsFresh) {
|
||||
// 超龄判定为严格大于：恰好等于有效期仍算新鲜
|
||||
EXPECT_FALSE(StalenessChecker::IsStale(800, 1000, 200));
|
||||
}
|
||||
|
||||
TEST(StalenessCheckerTest, InconsistentTimestampIsStale) {
|
||||
// C4：now < sample 时间戳不一致 → 年龄不可信 → 判超龄（宁降级不轻信）
|
||||
EXPECT_TRUE(StalenessChecker::IsStale(2000, 1000, 200));
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,349 @@
|
||||
// =============================================================================
|
||||
// tracking_control_law_test.cpp —— TrackingControlLaw 主控律单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §2.3 / §4 / §5 / §6 / §7 / §9）：
|
||||
// 超龄降级（姿态缺失/过期、视觉超龄、降级优先级）、判丢零速刹车、
|
||||
// Coast 清积分与标志、速率式/位置式航向通道（含绝对航向合成与回绕，
|
||||
// A2）、垂直统一速度支路（A1）、距离通道三种无距离降级动作、
|
||||
// 首拍变化率限幅（S6）、机体系→NED 输出旋转（A4）。
|
||||
// =============================================================================
|
||||
|
||||
#include <cmath>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/tracking_control_law.h"
|
||||
#include "visual_track_control/vtc_config.h"
|
||||
#include "visual_track_control/vtc_types.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using namespace drone::vtc;
|
||||
|
||||
constexpr double kPi = 3.14159265358979323846;
|
||||
constexpr double kDegToRad = kPi / 180.0;
|
||||
constexpr double kRadToDeg = 180.0 / kPi;
|
||||
|
||||
/// 测试基准配置：大限幅使限幅器近似透明，便于独立验证各通道数学。
|
||||
/// 画面 1000×1000、视场角 90° → 针孔系数 2·tan(45°)/1000 = 0.002，
|
||||
/// 偏移 100 像素 → 视线角 atan(0.2) ≈ 11.3099°。
|
||||
TrackingConfig MakeConfig() {
|
||||
TrackingConfig cfg;
|
||||
cfg.control.frequency_hz = 20.0;
|
||||
cfg.control.visual_stale_ms = 200;
|
||||
cfg.control.radar_stale_ms = 500;
|
||||
cfg.control.attitude_stale_ms = 300;
|
||||
cfg.camera.image_width = 1000;
|
||||
cfg.camera.image_height = 1000;
|
||||
cfg.camera.fov_h_deg = 90.0;
|
||||
cfg.camera.fov_v_deg = 90.0;
|
||||
cfg.heading.mode = HeadingMode::kRate;
|
||||
cfg.heading.gain = 1.0;
|
||||
cfg.heading.yaw_rate_limit_dps = 30.0;
|
||||
cfg.heading.yaw_slew_limit_dps2 = 100000.0;
|
||||
cfg.vertical.vz_gain_mps_per_deg = 0.1;
|
||||
cfg.vertical.vz_limit_mps = 2.0;
|
||||
cfg.distance.d_exp_m = 10.0;
|
||||
cfg.distance.kp = 0.5;
|
||||
cfg.distance.ki = 0.0;
|
||||
cfg.distance.kd = 0.0;
|
||||
cfg.distance.integral_limit = 0.0;
|
||||
cfg.distance.derivative_filter_coef = 0.0;
|
||||
cfg.distance.approach_velocity_limit_mps = 3.0;
|
||||
cfg.distance.retreat_velocity_limit_mps = 1.0;
|
||||
cfg.distance.no_distance_action = NoDistanceAction::kHold;
|
||||
cfg.distance.no_distance_approach_limit_mps = 0.5;
|
||||
cfg.accel_limit.ax_mps2 = 1000.0;
|
||||
cfg.accel_limit.ay_mps2 = 1000.0;
|
||||
cfg.accel_limit.az_mps2 = 1000.0;
|
||||
return cfg;
|
||||
}
|
||||
|
||||
/// 新鲜快照：目标在画面中心，姿态/追踪/雷达均新鲜
|
||||
ControlSnapshot MakeSnapshot(std::int64_t now_ms) {
|
||||
ControlSnapshot s;
|
||||
s.track.tracked = true;
|
||||
s.track.is_predicted = false;
|
||||
s.track.center = {500.0, 500.0};
|
||||
s.track_time_ms = now_ms;
|
||||
s.attitude_present = true;
|
||||
s.attitude_time_ms = now_ms;
|
||||
return s;
|
||||
}
|
||||
|
||||
// ---- 超龄降级（§7.1 / §9.2） ----
|
||||
|
||||
TEST(TrackingControlLawTest, AttitudeMissingDegrades) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.attitude_present = false;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
|
||||
EXPECT_FALSE(out.valid);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, AttitudeStaleDegrades) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.attitude_time_ms = 600; // 400 ms 前 > 300 ms 阈值
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, VisionStaleDegrades) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track_time_ms = 700; // 300 ms 前 > 200 ms 阈值
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, NeverSampledTrackDegrades) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track_time_ms = 0; // 从未取样
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kDegradedHold);
|
||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kVisionStale);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, AttitudeStaleBeatsVisionStale) {
|
||||
// 降级优先级：姿态校验在前（§9.2 姿态过期禁止一切空间转换）
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.attitude_present = false;
|
||||
snap.track_time_ms = 0;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.degraded_reason, DegradedReason::kAttitudeStale);
|
||||
}
|
||||
|
||||
// ---- 判丢刹车（§6.4） ----
|
||||
|
||||
TEST(TrackingControlLawTest, LostTargetBrakes) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.tracked = false;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kBrakeHover);
|
||||
EXPECT_TRUE(out.valid);
|
||||
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
||||
EXPECT_DOUBLE_EQ(out.vy_mps, 0.0);
|
||||
EXPECT_DOUBLE_EQ(out.vz_mps, 0.0);
|
||||
}
|
||||
|
||||
// ---- Coast（§6.4） ----
|
||||
|
||||
TEST(TrackingControlLawTest, CoastSetsFlagAndKeepsTracking) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.is_predicted = true;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
||||
EXPECT_TRUE(out.coast_active);
|
||||
EXPECT_TRUE(out.valid);
|
||||
}
|
||||
|
||||
// ---- 水平航向通道（§4.2，A2） ----
|
||||
|
||||
TEST(TrackingControlLawTest, RateModeMapsBetaToYawRate) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {600.0, 500.0}; // 目标偏右 100 像素
|
||||
const auto out = law.Update(snap, 1000);
|
||||
const double beta_deg = std::atan(0.2) * kRadToDeg; // ≈ 11.31°
|
||||
EXPECT_NEAR(out.yaw_rate_dps, beta_deg, 1e-6); // gain=1，右转正
|
||||
EXPECT_DOUBLE_EQ(out.yaw_deg, 0.0);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, RateModeClampsToYawRateLimit) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {950.0, 500.0}; // 大偏移：β≈43.5° > 30°/s 上限
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_NEAR(out.yaw_rate_dps, 30.0, 1e-9);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, PositionModeComposesAbsoluteHeading) {
|
||||
// A2：位置式输出为绝对 NED 航向 = 当前航向 + 限幅修正量
|
||||
auto cfg = MakeConfig();
|
||||
cfg.heading.mode = HeadingMode::kPosition;
|
||||
TrackingControlLaw law(cfg);
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {600.0, 500.0};
|
||||
snap.yaw_rad = 45.0 * kDegToRad;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
const double beta_deg = std::atan(0.2) * kRadToDeg;
|
||||
EXPECT_NEAR(out.yaw_deg, 45.0 + beta_deg, 1e-6);
|
||||
EXPECT_DOUBLE_EQ(out.yaw_rate_dps, 0.0);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, PositionModeWrapsAround180) {
|
||||
auto cfg = MakeConfig();
|
||||
cfg.heading.mode = HeadingMode::kPosition;
|
||||
TrackingControlLaw law(cfg);
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {600.0, 500.0};
|
||||
snap.yaw_rad = 175.0 * kDegToRad;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
const double beta_deg = std::atan(0.2) * kRadToDeg;
|
||||
// 175 + 11.31 = 186.31 → 回绕到 ≈ -173.69
|
||||
EXPECT_NEAR(out.yaw_deg, 175.0 + beta_deg - 360.0, 1e-6);
|
||||
EXPECT_GT(out.yaw_deg, -180.0);
|
||||
EXPECT_LT(out.yaw_deg, 180.0);
|
||||
}
|
||||
|
||||
// ---- 垂直统一速度支路（§5，A1） ----
|
||||
|
||||
TEST(TrackingControlLawTest, VerticalOffsetDrivesVz) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {500.0, 600.0}; // 目标偏低 100 像素 → α>0 → 下降
|
||||
const auto out = law.Update(snap, 1000);
|
||||
const double alpha_deg = std::atan(0.2) * kRadToDeg;
|
||||
EXPECT_NEAR(out.vz_mps, 0.1 * alpha_deg, 1e-6);
|
||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading); // A1：不切姿态微调模式
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, VerticalVelocityClamped) {
|
||||
TrackingControlLaw law(MakeConfig());
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.track.center = {500.0, 990.0}; // α≈44.5° × 0.1 = 4.45 > 2 上限
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_NEAR(out.vz_mps, 2.0, 1e-9);
|
||||
}
|
||||
|
||||
// ---- 距离通道（§6.2 / §6.3） ----
|
||||
|
||||
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;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_NEAR(out.vx_mps, 3.0, 1e-9);
|
||||
}
|
||||
|
||||
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;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_NEAR(out.vx_mps, -1.0, 1e-9);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, NoDistanceHoldZerosVx) {
|
||||
TrackingControlLaw law(MakeConfig()); // 缺省 kHold
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.associated_to_target = false;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
||||
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, NoDistanceSlowApproach) {
|
||||
auto cfg = MakeConfig();
|
||||
cfg.distance.no_distance_action = NoDistanceAction::kSlowApproach;
|
||||
TrackingControlLaw law(cfg);
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.associated_to_target = false;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_NEAR(out.vx_mps, 0.5, 1e-9);
|
||||
}
|
||||
|
||||
TEST(TrackingControlLawTest, NoDistanceExitDegrades) {
|
||||
auto cfg = MakeConfig();
|
||||
cfg.distance.no_distance_action = NoDistanceAction::kExit;
|
||||
TrackingControlLaw law(cfg);
|
||||
auto snap = MakeSnapshot(1000);
|
||||
snap.associated_to_target = 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) {
|
||||
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 阈值 → 距离不可用
|
||||
const auto out = law.Update(snap, 1000);
|
||||
EXPECT_EQ(out.mode, ControlMode::kVelocityHeading);
|
||||
EXPECT_DOUBLE_EQ(out.vx_mps, 0.0);
|
||||
}
|
||||
|
||||
// ---- 首拍/限幅（§8、S6） ----
|
||||
|
||||
TEST(TrackingControlLawTest, FirstTickVxSlewsFromZero) {
|
||||
// S6：首条速度指令必须经变化率限幅，从 0 渐变而非阶跃
|
||||
auto cfg = MakeConfig();
|
||||
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;
|
||||
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
|
||||
}
|
||||
|
||||
// ---- 机体系 → NED 旋转（§2.3，A4） ----
|
||||
|
||||
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;
|
||||
const auto out = law.Update(snap, 1000);
|
||||
// 机体前向 3.0 → 东向 3.0，北向 ≈ 0
|
||||
EXPECT_NEAR(out.vx_mps, 0.0, 1e-9);
|
||||
EXPECT_NEAR(out.vy_mps, 3.0, 1e-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;
|
||||
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);
|
||||
}
|
||||
|
||||
// ---- Reset（§6.4） ----
|
||||
|
||||
TEST(TrackingControlLawTest, ResetRestoresSlewFromZero) {
|
||||
auto cfg = MakeConfig();
|
||||
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;
|
||||
for (std::int64_t t = 1000; t < 3000; t += 50) {
|
||||
snap.track_time_ms = t;
|
||||
snap.radar_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.attitude_time_ms = 3000;
|
||||
const auto out = law.Update(snap, 3000);
|
||||
// Reset 后限幅器归零：又从 0.1 开始渐变
|
||||
EXPECT_NEAR(out.vx_mps, 0.1, 1e-9);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,224 @@
|
||||
// =============================================================================
|
||||
// vtc_config_test.cpp —— TrackingConfig::LoadFromJson 单元测试
|
||||
//
|
||||
// 覆盖（物理追踪思路 §8、需求分析 NFR-007）：
|
||||
// 真实配置文件加载成功与取值正确（含 A1/A2 修改后的字段结构）、
|
||||
// 缺字段 / 缺分组 / 非法枚举 / 非法数值 / 文件缺失均抛带字段名的异常。
|
||||
//
|
||||
// 依赖编译期宏：
|
||||
// VTC_TEST_CONFIG —— config/track_control.json 绝对路径
|
||||
// VTC_TEST_TMP_DIR —— 测试临时文件输出目录（构建目录）
|
||||
// =============================================================================
|
||||
|
||||
#include <cstdio>
|
||||
#include <fstream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "visual_track_control/vtc_config.h"
|
||||
|
||||
namespace {
|
||||
|
||||
using drone::vtc::HeadingMode;
|
||||
using drone::vtc::NoDistanceAction;
|
||||
using drone::vtc::TrackingConfig;
|
||||
|
||||
/// 与真实配置同构的完整合法 JSON（用于派生各非法变体）
|
||||
const char* kFullJson = R"({
|
||||
"control": {
|
||||
"frequency_hz": 20.0,
|
||||
"visual_stale_ms": 200,
|
||||
"radar_stale_ms": 500,
|
||||
"attitude_stale_ms": 300
|
||||
},
|
||||
"camera": {
|
||||
"image_width": 1920,
|
||||
"image_height": 1080,
|
||||
"fov_h_deg": 60.0,
|
||||
"fov_v_deg": 35.0
|
||||
},
|
||||
"heading": {
|
||||
"mode": "rate",
|
||||
"gain": 1.0,
|
||||
"yaw_rate_limit_dps": 30.0,
|
||||
"yaw_slew_limit_dps2": 60.0
|
||||
},
|
||||
"vertical": {
|
||||
"vz_gain_mps_per_deg": 0.1,
|
||||
"vz_limit_mps": 2.0
|
||||
},
|
||||
"distance": {
|
||||
"d_exp_m": 10.0,
|
||||
"kp": 0.5,
|
||||
"ki": 0.05,
|
||||
"kd": 0.1,
|
||||
"integral_limit": 2.0,
|
||||
"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
|
||||
},
|
||||
"accel_limit": {
|
||||
"ax_mps2": 2.0,
|
||||
"ay_mps2": 2.0,
|
||||
"az_mps2": 1.5
|
||||
}
|
||||
})";
|
||||
|
||||
/// 在 json 中把首个 from 替换为 to（未找到即测试失败）
|
||||
void ReplaceOnce(std::string& json, const std::string& from, const std::string& to) {
|
||||
const auto pos = json.find(from);
|
||||
ASSERT_NE(pos, std::string::npos) << "未找到待替换片段: " << from;
|
||||
json.replace(pos, from.size(), to);
|
||||
}
|
||||
|
||||
/// 把 JSON 文本写入临时文件并返回路径（调用方用完后删除）
|
||||
std::string WriteTempJson(const std::string& content, const char* name) {
|
||||
const std::string path = std::string(VTC_TEST_TMP_DIR) + "/" + name;
|
||||
std::ofstream ofs(path);
|
||||
EXPECT_TRUE(static_cast<bool>(ofs)) << "无法写入临时配置: " << path;
|
||||
ofs << content;
|
||||
return path;
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, LoadRealConfigSucceeds) {
|
||||
const TrackingConfig cfg = TrackingConfig::LoadFromJson(VTC_TEST_CONFIG);
|
||||
EXPECT_DOUBLE_EQ(cfg.control.frequency_hz, 20.0);
|
||||
EXPECT_EQ(cfg.control.visual_stale_ms, 200);
|
||||
EXPECT_EQ(cfg.camera.image_width, 1920);
|
||||
EXPECT_EQ(cfg.camera.image_height, 1080);
|
||||
EXPECT_DOUBLE_EQ(cfg.camera.fov_h_deg, 60.0);
|
||||
EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate); // A2：默认速率式
|
||||
EXPECT_DOUBLE_EQ(cfg.heading.yaw_rate_limit_dps, 30.0);
|
||||
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_DOUBLE_EQ(cfg.accel_limit.ax_mps2, 2.0);
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, FullJsonRoundTripMatchesExpected) {
|
||||
// 内嵌合法 JSON 与真实配置文件行为一致
|
||||
const std::string path = WriteTempJson(kFullJson, "vtc_cfg_full.json");
|
||||
const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
|
||||
EXPECT_EQ(cfg.heading.mode, HeadingMode::kRate);
|
||||
EXPECT_DOUBLE_EQ(cfg.distance.kp, 0.5);
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, PositionModeAccepted) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"position\"");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_position.json");
|
||||
const TrackingConfig cfg = TrackingConfig::LoadFromJson(path);
|
||||
EXPECT_EQ(cfg.heading.mode, HeadingMode::kPosition);
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, MissingFileThrowsWithPath) {
|
||||
try {
|
||||
TrackingConfig::LoadFromJson("/nonexistent/vtc_no_such_file.json");
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("vtc_no_such_file.json"),
|
||||
std::string::npos);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, MalformedJsonThrows) {
|
||||
const std::string path = WriteTempJson("{ this is not json", "vtc_cfg_bad.json");
|
||||
EXPECT_THROW(TrackingConfig::LoadFromJson(path), std::runtime_error);
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, MissingFieldThrowsWithFieldName) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"gain\": 1.0,\n", "");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_missing.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("heading.gain"), std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, MissingSectionThrows) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json,
|
||||
",\n \"accel_limit\": {\n"
|
||||
" \"ax_mps2\": 2.0,\n"
|
||||
" \"ay_mps2\": 2.0,\n"
|
||||
" \"az_mps2\": 1.5\n"
|
||||
" }",
|
||||
"");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_nosection.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("accel_limit"), std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, InvalidHeadingModeThrows) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"mode\": \"rate\"", "\"mode\": \"magic\"");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_badmode.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("heading.mode"), std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, InvalidNoDistanceActionThrows) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"no_distance_action\": \"hold\"",
|
||||
"\"no_distance_action\": \"zoom\"");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_badaction.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("no_distance_action"),
|
||||
std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, NegativeFrequencyThrows) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"frequency_hz\": 20.0", "\"frequency_hz\": -1.0");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_negfreq.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("control.frequency_hz"),
|
||||
std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
TEST(VtcConfigTest, FovOutOfRangeThrows) {
|
||||
std::string json = kFullJson;
|
||||
ReplaceOnce(json, "\"fov_h_deg\": 60.0", "\"fov_h_deg\": 200.0");
|
||||
const std::string path = WriteTempJson(json, "vtc_cfg_badfov.json");
|
||||
try {
|
||||
TrackingConfig::LoadFromJson(path);
|
||||
FAIL() << "应当抛出异常";
|
||||
} catch (const std::runtime_error& e) {
|
||||
EXPECT_NE(std::string(e.what()).find("camera.fov_h_deg"), std::string::npos);
|
||||
}
|
||||
std::remove(path.c_str());
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,443 @@
|
||||
# 视觉跟踪控制律实现文档（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）。
|
||||
- **统一限幅**（规格 §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 视为从未取样，直接判超龄** |
|
||||
| `radar_distance_m` | 机头前向单点距离（米） |
|
||||
| `associated_to_target` | 相机—雷达空间关联成立标志；**未关联不得当目标距离**（规格 §6.3、需求分析 FR-041） |
|
||||
| `radar_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`、
|
||||
`radar_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`
|
||||
（`ax_mps2`、`ay_mps2`、`az_mps2`）。JSON 键名与字段一一对应，见
|
||||
`config/track_control.json`。`heading.mode` 默认速率式（`"rate"`）；
|
||||
相机组原 `linear_ratio` 键已随线性/针孔双路一并移除（§3.5）。
|
||||
|
||||
### 2.3 ICommandTransport 契约
|
||||
|
||||
```cpp
|
||||
class ICommandTransport {
|
||||
public:
|
||||
virtual bool Send(const std::uint8_t* data, std::size_t len) = 0;
|
||||
};
|
||||
```
|
||||
|
||||
- **只吃字节流**：串口、电台、回环打印均可注入实现；后续重写传输层
|
||||
只换实现，不动打包与控制律；
|
||||
- 返回 `true` = 已交付底层链路，`false` = 发送失败（调用方可计数/告警）；
|
||||
- 实现方保证不长期持有 `data` 指针（同步拷贝或同步写出）。
|
||||
|
||||
### 2.4 数据流向
|
||||
|
||||
```text
|
||||
推理侧每帧: 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·α` → 加速度限幅 → 速度限幅）
|
||||
→ ⑦ 距离通道（关联成立且未超龄才可用；否则清积分并按
|
||||
`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(当前航向 + 限幅修正量)`；速率式
|
||||
（默认）输出角速度指令，天然相对量、无此问题。
|
||||
|
||||
### 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）。
|
||||
|
||||
```bash
|
||||
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 无距离降级路径（hold：
|
||||
前向清零）；
|
||||
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 标志、速率/位置两种航向模式（含绝对航向回绕）、垂直支路与限幅、距离通道四种降级动作、雷达超龄回落、首拍变化率限幅（S6）、NED 旋转、`Reset` 后从零渐变 |
|
||||
| `vtc_config_test.cpp` | 真实配置加载、position 模式可接受、缺文件/缺字段/缺分组/类型错/枚举非法/数值越界均抛异常且消息携带字段名 |
|
||||
| `mavlink_command_gateway_test.cpp` | 手工解码 MAVLink v2 字节流（不含 `mavlink.h`，构建约束见 §3.6）：帧头、ID 84、三种掩码分支（含"双零 → 速率掩码"回归）、坐标基准字节、目标系统/组件字节、传输失败传播 |
|
||||
|
||||
运行方式（构建后）：
|
||||
|
||||
```bash
|
||||
./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. **雷达关联标志来源**：`associated_to_target` 必须由感知融合层按
|
||||
规格 §6.3 / 需求分析 FR-041 的判据给出，控制律不自行关联。
|
||||
|
||||
### 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
Block a user