first commit
This commit is contained in:
@@ -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
|
||||
Reference in New Issue
Block a user