# code-test-drone

This project contains a C++ reference implementation of a visual target tracking and control system and its Python port, designed for drone tracking and control of moving targets.

## Project Structure

```
code-test-drone/
├── reference-c++/           # C++ reference implementation
│   ├── target_tracker/      # Target tracking library (independent sub-project)
│   │   ├── include/         # Header files directory
│   │   ├── src/             # Source files directory
│   │   └── tests/           # Unit tests
│   └── visual_track_control/# Visual tracking control library
│       ├── include/         # Header files directory
│       ├── src/             # Source files directory
│       └── tests/           # Unit tests
└── utils/                   # Python port
    ├── target_tracker/      # Target tracking (Python version)
    ├── visual_track_control/# Tracking control (Python version)
    └── tests/               # Unit tests
```

## Main Modules

### target_tracker - Target Tracking Library

Provides target tracking functionality based on Kalman filtering:

- **KalmanBoxFilter**: 8-dimensional Kalman filter (position, velocity)
- **Tracklet**: Target trajectory management and state updates
- **TargetTracker**: Main tracker, containing association and election logic

Core features:
- Greedy association algorithm for detection matching
- Three-level fallback mechanism for main target election
- Supports Coast (pure prediction) and Lost (missed) state management

### visual_track_control - Visual Tracking Control Library

Generates drone control commands based on tracking results:

- **PIDController**: Distance PID control
- **TrackingControlLaw**: Tracking control law
- **MavlinkCommandGateway**: MAVLink command gateway
- **PixelToAngle**: Pixel to angle conversion
- **StalenessChecker**: Data staleness check
- **SlewRateLimiter**: Rate limiter

Supports multiple control modes:
- **Rate Mode**: Angular rate control
- **Position Mode**: Absolute heading control
- **Distance Control**: Approach/retreat strategy

## Environment Requirements

### C++ Version
- CMake 3.10+
- C++17 Compiler
- MAVLink Library (MAVSDK)

### Python Version
- Python 3.12+
- Dependencies: `numpy`, `pymavlink`, `mavsdk`

## Installation

### Python Environment
```bash
pip install -r requirements.txt
```

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

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

## Testing

### Python Tests
```bash
cd utils
pytest
```

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

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

## Configuration

Both C++ and Python versions use JSON format configuration files:

```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
    }
}
```

## Usage Examples

### Python Tracking Demo
```bash
cd utils
python demo_track_control.py
```

### C++ Tracking Demo
```bash
cd reference-c++/visual_track_control/build
./track_control_demo
```

## Porting Notes

Please refer to the README in each subdirectory for interface differences between the Python version and the C++ version:
- [target_tracker Python Version](./utils/target_tracker/README.md)
- [visual_track_control Python Version](./utils/visual_track_control/README.md)

## License

This project follows open source license agreements.

## Contributing Guidelines

Feel free to submit Issues and Pull Requests to improve this project.