# ArduPilot + Gazebo Guide

## Overview

This project uses:
- **ArduPilot SITL**: Software-in-the-loop flight controller
- **Gazebo**: 3D physics simulation
- **MAVLink**: Communication protocol

## Architecture

```
┌─────────────┐     JSON (UDP 9002)    ┌─────────────┐    MAVLink (TCP 5760)    ┌─────────────┐
│   Gazebo    │◄──────────────────────►│ ArduPilot   │◄────────────────────────►│   Your      │
│  (Physics)  │     sensor data        │    SITL     │     commands/telemetry   │ Controller  │
└─────────────┘                        └─────────────┘                          └─────────────┘
```

## Quick Start

### Step 1: Start Gazebo

```bash
cd ~/RDC_Simulation
./scripts/run_ardupilot_sim.sh runway
```

Wait until the drone appears in the 3D view.

### Step 2: Start SITL

```bash
source ~/venv-ardupilot/bin/activate
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console
```

You should see telemetry scrolling (NOT "No JSON sensor message").

### Step 3: Run Controller

```bash
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation
python scripts/run_ardupilot.py --pattern square
```

## Flight Patterns

```bash
# Square pattern (5m sides, 5m altitude)
python scripts/run_ardupilot.py --pattern square --size 5 --altitude 5

# Circle pattern (5m radius)
python scripts/run_ardupilot.py --pattern circle --size 5

# Hover in place for testing
python scripts/run_ardupilot.py --pattern hover

# Larger pattern at higher altitude
python scripts/run_ardupilot.py --pattern square --size 10 --altitude 15
```

## Controller Code

The flight controller is in `src/drone_controller.py`:

```python
from src.drone_controller import SimpleDroneController

drone = SimpleDroneController()
drone.connect()
drone.set_mode("GUIDED")
drone.arm()
drone.takeoff(5.0)
drone.fly_square(size=5, altitude=5)
drone.land()
```

### Key Methods

| Method | Description |
|--------|-------------|
| `connect()` | Connect to SITL via MAVLink |
| `set_mode(mode)` | Set flight mode (GUIDED, LAND, etc.) |
| `arm()` | Arm motors (force arm) |
| `takeoff(alt)` | Take off to altitude |
| `goto(x, y, z)` | Go to position (NED frame) |
| `fly_to_and_wait(x, y, alt)` | Go to position and wait |
| `fly_square(size, alt)` | Fly square pattern |
| `fly_circle(radius, alt)` | Fly circle pattern |
| `land()` | Land the drone |

## Available Worlds

```bash
# Outdoor runway (default)
./scripts/run_ardupilot_sim.sh runway

# Indoor warehouse
./scripts/run_ardupilot_sim.sh warehouse
```

## MAVLink Connection

The controller connects via TCP port 5760:

```python
# config.py
MAVLINK = {
    "connection_string": "tcp:127.0.0.1:5760",
}
```

## Troubleshooting

### "No JSON sensor message received"

SITL isn't receiving data from Gazebo.

**Fix:**
1. Make sure Gazebo is running FIRST
2. Wait for the world to fully load
3. Then start SITL

### Drone won't arm

**Fix:**
```bash
# Check pre-arm status in MAVProxy
arm status

# Force arm
arm throttle force
```

### Wrong Python environment

**Fix:**
```bash
source ~/venv-ardupilot/bin/activate
which python3  # Should be ~/venv-ardupilot/bin/python3
```

### Gazebo plugin not loading

**Fix:**
```bash
# Check plugin exists
ls ~/ardupilot_gazebo/build/libArduPilotPlugin.so

# Check paths
echo $GZ_SIM_SYSTEM_PLUGIN_PATH
# Should include: ~/ardupilot_gazebo/build
```

## Manual SITL Control

Use MAVProxy commands:

```bash
# In SITL console
mode GUIDED
arm throttle force
takeoff 5

# Move to position
guided 10 0 -5

# Land
mode LAND
```

## Useful Commands

```bash
# List Gazebo topics
gz topic -l

# Check SITL status
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --map

# View camera (if available)
gz topic -e -t /camera
```