# ArduPilot + Gazebo Guide

## Architecture

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

## Quick Start (3 Terminals)

### Terminal 1: Start Gazebo FIRST

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

Wait until the drone appears in the 3D view!

### Terminal 2: Start ArduCopter SITL

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

You should see:
- "JSON received: timestamp, imu, position..." ✅
- NOT "No JSON sensor message received" ❌

### Terminal 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
python scripts/run_ardupilot.py --pattern hover

# GPS mode (instead of GPS-denied)
python scripts/run_ardupilot.py --pattern square --gps
```

## Controller Code

```python
from src.drone_controller import DroneController

drone = DroneController()
drone.connect()

# GPS-denied setup
drone.setup_gps_denied()
drone.set_mode("GUIDED_NOGPS")

# Or GPS mode
# drone.set_mode("GUIDED")

drone.arm()
drone.takeoff(5.0)
drone.fly_square(size=5, altitude=5)
drone.land()
```

## ArduCopter Flight Modes

| Mode | ID | Description |
|------|-----|-------------|
| STABILIZE | 0 | Manual, self-leveling |
| ALT_HOLD | 2 | Hold altitude |
| GUIDED | 4 | Waypoint commands (GPS required) |
| LOITER | 5 | Hold position (GPS required) |
| RTL | 6 | Return to launch |
| LAND | 9 | Automatic landing |
| **GUIDED_NOGPS** | 20 | Commands without GPS |

## MAVProxy Commands

In the SITL console, you can type:

```bash
# Check status
status

# Change mode
mode GUIDED
mode GUIDED_NOGPS

# Arm
arm throttle force

# Takeoff (in GUIDED mode)
takeoff 5

# Land
mode LAND

# Set parameter
param set ARMING_CHECK 0
```

## Gazebo Worlds

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

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

## MAVLink Connection

Default: `tcp:127.0.0.1:5760`

Port map:
- **5760**: Primary MAVLink (SERIAL0)
- **5762**: Secondary (SERIAL1)
- **5763**: Tertiary (SERIAL2)
- **14550**: MAVProxy GCS output (UDP)

## Troubleshooting

### "No JSON sensor message received"

Gazebo isn't sending data.

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

### Drone won't arm

**Fix:**
```bash
# In MAVProxy console
arm throttle force

# Or set in your code
drone.set_param("ARMING_CHECK", 0)
```

### Wrong vehicle type (ArduPlane)

Mode list shows FBWA, FBWB, QSTABILIZE instead of GUIDED_NOGPS, ALT_HOLD.

**Fix:**
```bash
pkill -9 -f sim_vehicle
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --wipe-eeprom
```

### Mode not changing

The controller hardcodes mode IDs for ArduCopter. If using ArduPlane, modes will differ.

### Gazebo plugin not loading

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

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

## Useful Debug Commands

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

# Echo a topic
gz topic -e -t /model/iris_with_gimbal/pose

# Check SITL processes
ps aux | grep ardupilot

# Check port usage
lsof -i :5760
```