# code-test-drone

本项目包含一个视觉目标追踪与控制系统的 C++ 参考实现及其 Python 移植版本，用于无人机对移动目标的追踪与控制。

## 项目结构

```
code-test-drone/
├── reference-c++/           # C++ 参考实现
│   ├── target_tracker/      # 目标追踪库（独立子工程）
│   │   ├── include/         # 头文件目录
│   │   ├── src/             # 源文件目录
│   │   └── tests/           # 单元测试
│   └── visual_track_control/# 视觉追踪控制库
│       ├── include/         # 头文件目录
│       ├── src/             # 源文件目录
│       └── tests/           # 单元测试
└── utils/                   # Python 移植
    ├── target_tracker/      # 目标追踪（Python 版）
    ├── visual_track_control/# 追踪控制（Python 版）
    └── tests/               # 单元测试
```

## 主要模块

### target_tracker - 目标追踪库

提供基于卡尔曼滤波的目标追踪功能：

- **KalmanBoxFilter**: 8维卡尔曼滤波器（位置、速度）
- **Tracklet**: 目标轨迹管理与状态更新
- **TargetTracker**: 主追踪器，包含关联、选举逻辑

核心特性：
- 贪婪关联算法进行检测匹配
- 主目标选举三级回退机制
- 支持 Coast（纯预测）与 Lost（丢失）状态管理

### visual_track_control - 视觉追踪控制库

基于追踪结果生成无人机控制指令：

- **PIDController**: 距离 PID 控制
- **TrackingControlLaw**: 追踪控制律
- **MavlinkCommandGateway**: MAVLink 指令网关
- **PixelToAngle**: 像素到角度转换
- **StalenessChecker**: 数据时效性检查
- **SlewRateLimiter**: 速率限制器

支持多种控制模式：
- **Rate 模式**: 角速率控制
- **Position 模式**: 绝对航向控制
- **距离控制**: 接近/后退策略

## 环境要求

### C++ 版本
- CMake 3.10+
- C++17 编译器
- MAVLink 库（MAVSDK）

### Python 版本
- Python 3.12+
- 依赖：`numpy`, `pymavlink`, `mavsdk`

## 安装

### Python 环境
```bash
pip install -r requirements.txt
```

### C++ 构建
```bash
cd reference-c++/target_tracker
mkdir build && cd build
cmake ..
make

cd ../../visual_track_control
mkdir build && cd build
cmake ..
make
```

## 测试

### Python 测试
```bash
cd utils
pytest
```

### C++ 测试
```bash
cd reference-c++/target_tracker/build
ctest

cd ../../visual_track_control/build
ctest
```

## 配置

C++ 与 Python 版本均使用 JSON 格式配置文件：

```json
{
    "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
    },
    "distance": {
        "d_exp_m": 10.0,
        "kp": 0.5,
        "ki": 0.05,
        "kd": 0.1,
        "integral_limit": 2.0
    }
}
```

## 使用示例

### Python 追踪演示
```bash
cd utils
python demo_track_control.py
```

### C++ 追踪演示
```bash
cd reference-c++/visual_track_control/build
./track_control_demo
```

## 移植说明

Python 版本与 C++ 版的接口差异请参考各子目录的 README：
- [target_tracker Python 版](./utils/target_tracker/README.md)
- [visual_track_control Python 版](./utils/visual_track_control/README.md)

## 许可证

本项目遵循开源许可证协议。

## 贡献指南

欢迎提交 Issue 和 Pull Request 改进本项目。