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