# ArduPilot SITL + Gazebo Integration

This guide explains how to run the drone simulation with ArduPilot Software-In-The-Loop (SITL) and MAVProxy, providing a realistic flight controller stack.

## Overview

The ArduPilot integration replaces the simple velocity control with a full ArduPilot flight stack:

```
┌──────────────────┐     ┌─────────────────┐     ┌──────────────────┐
│  ArduPilot SITL  │◄───►│  Gazebo + Plugin │◄───►│  MAVLink Bridge  │
│  (Flight Control)│ JSON│  (Physics Sim)   │ ROS │  + Controllers   │
└──────────────────┘     └─────────────────┘     └──────────────────┘
        ▲                                                    │
        │ UDP                                               │
        │                                                    ▼
┌──────────────────┐                              ┌──────────────────┐
│    MAVProxy      │◄────────────────────────────►│ DroneController  │
│    (GCS)         │     MAVLink Commands         │ (Your Algorithm) │
└──────────────────┘                              └──────────────────┘
```

## Components

| Component | Description |
|-----------|-------------|
| **ArduPilot SITL** | Full autopilot firmware running in simulation |
| **ardupilot_gazebo** | Plugin connecting Gazebo physics to ArduPilot |
| **MAVProxy** | Ground Control Station for monitoring/commands |
| **MAVLink Bridge** | ROS 2 node bridging MAVLink ↔ ROS topics |
| **Drone Controller** | Your landing algorithm |

## Prerequisites

### 1. ArduPilot SITL

Install ArduPilot development environment:

```bash
# Ubuntu/Debian
git clone https://github.com/ArduPilot/ardupilot.git ~/ardupilot
cd ~/ardupilot
git submodule update --init --recursive
Tools/environment_install/install-prereqs-ubuntu.sh -y
. ~/.profile

# Set environment
echo 'export PATH=$PATH:$HOME/ardupilot/Tools/autotest' >> ~/.bashrc
echo 'export ARDUPILOT_HOME=$HOME/ardupilot' >> ~/.bashrc
source ~/.bashrc
```

### 2. ArduPilot Gazebo Plugin

Install the ardupilot_gazebo plugin:

```bash
# For Gazebo Garden/Harmonic
git clone https://github.com/ArduPilot/ardupilot_gazebo.git ~/ardupilot_gazebo
cd ~/ardupilot_gazebo
mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4

# Add to Gazebo plugin path
echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH' >> ~/.bashrc
echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH' >> ~/.bashrc
source ~/.bashrc
```

### 3. pymavlink

```bash
pip install pymavlink
```

## Quick Start

### Option 1: Integrated Launch (Recommended)

This starts everything together:

```bash
# Terminal 1: Start Gazebo
ros2 launch gazebo/launch/ardupilot_drone.launch.py

# Terminal 2: Start SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map

# Terminal 3: Run bridge + controllers
python run_ardupilot.py --no-sitl --pattern circular
```

### Option 2: Manual Setup

```bash
# Terminal 1: Start Gazebo world
gz sim -r gazebo/worlds/ardupilot_drone.sdf

# Terminal 2: Start ArduPilot SITL
cd ~/ardupilot
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console

# Terminal 3: Run MAVLink bridge + controllers
python run_ardupilot.py --no-sitl
```

### Option 3: Full Automatic

```bash
# Starts everything (requires SITL installed)
python run_ardupilot.py --pattern circular --console --map
```

## Flight Operations

### Using MAVProxy Commands

Once connected, use MAVProxy to control the drone:

```bash
# Set GUIDED mode for algorithm control
mode guided

# Arm motors
arm throttle

# Take off to 5 meters
takeoff 5

# Land
mode land

# Disarm
disarm
```

### Using the MAVLink Bridge API

From Python, you can control the drone directly:

```python
from mavlink_bridge import MAVLinkBridge

# Create bridge
bridge = MAVLinkBridge(sitl_port=14550)

# Arm and takeoff
bridge.set_mode('GUIDED')
bridge.arm()
bridge.takeoff(altitude=5.0)

# Land
bridge.land()
```

## Files

| File | Description |
|------|-------------|
| `mavlink_bridge.py` | ROS 2 ↔ MAVLink bridge |
| `run_ardupilot.py` | Integrated launcher |
| `gazebo/worlds/ardupilot_drone.sdf` | Gazebo world with ArduPilot plugin |
| `gazebo/launch/ardupilot_drone.launch.py` | ROS 2 launch file |

## Configuration

Edit `config.py` to adjust ArduPilot settings:

```python
ARDUPILOT = {
    "vehicle": "ArduCopter",      # ArduCopter, ArduPlane, APMrover2
    "frame": "gazebo-iris",       # Gazebo frame
    "sitl_host": "127.0.0.1",
    "sitl_port": 5760,
    "mavproxy_port": 14550,
}

MAVLINK = {
    "system_id": 1,
    "component_id": 191,
    "heartbeat_timeout": 5.0,
}
```

## Telemetry Format

The MAVLink bridge publishes telemetry in the same format as other modes:

```json
{
    "imu": {
        "orientation": {"roll": 0.0, "pitch": 0.0, "yaw": 0.0},
        "angular_velocity": {"x": 0.0, "y": 0.0, "z": 0.0}
    },
    "altimeter": {
        "altitude": 5.0,
        "vertical_velocity": 0.0
    },
    "velocity": {"x": 0.0, "y": 0.0, "z": 0.0},
    "position": {"x": 0.0, "y": 0.0, "z": 5.0},
    "landing_pad": {
        "relative_x": 0.5,
        "relative_y": 0.2,
        "distance": 4.8,
        "confidence": 0.95
    },
    "battery": {"voltage": 12.6, "remaining": 100},
    "armed": true,
    "flight_mode": "GUIDED",
    "connected": true
}
```

## Troubleshooting

### SITL Not Starting

```bash
# Check if SITL is installed
which sim_vehicle.py

# Set ArduPilot path
export ARDUPILOT_HOME=~/ardupilot
export PATH=$PATH:$ARDUPILOT_HOME/Tools/autotest
```

### Gazebo Plugin Not Found

```bash
# Check plugin path
echo $GZ_SIM_SYSTEM_PLUGIN_PATH

# Verify plugin exists
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so
```

### No MAVLink Connection

```bash
# Check if SITL is listening
netstat -tuln | grep 14550

# Test with mavlink console
python -c "from pymavlink import mavutil; c = mavutil.mavlink_connection('udpin:127.0.0.1:14550'); print(c.wait_heartbeat())"
```

### Drone Won't Arm

Common issues:
1. **Pre-arm checks failing** - Check MAVProxy console for errors
2. **GPS required** - In simulation, you may need to wait for GPS lock
3. **EKF not ready** - Wait for EKF to initialize

Disable pre-arm checks for testing (not recommended for real flights):
```
# In MAVProxy
param set ARMING_CHECK 0
```

## Flight Modes

| Mode | Description |
|------|-------------|
| **GUIDED** | Accept velocity/position commands from controller |
| **LOITER** | Hold position (GPS required) |
| **ALT_HOLD** | Maintain altitude, manual horizontal |
| **LAND** | Automatic landing |
| **STABILIZE** | Attitude stabilization only |

For autonomous landing, use **GUIDED** mode.

## Architecture Comparison

| Feature | Simple Gazebo | ArduPilot + Gazebo |
|---------|--------------|-------------------|
| Flight Controller | Velocity control | Full ArduPilot |
| Stabilization | Manual PD | Inbuilt EKF + PID |
| Flight Modes | None | All ArduPilot modes |
| Arming | Not required | Safety checks |
| Failsafes | None | Battery, GPS, etc. |
| MAVLink | No | Full protocol |
| GCS Support | No | QGC, Mission Planner |
| Realism | Low | High |