# Drone Controller Guide

## Overview

The `DroneController` class (`src/drone_controller.py`) provides a simple interface for controlling the drone in ArduPilot SITL simulation.

## Command Line Usage

```bash
# Activate environment first
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation

# Run flight patterns
python scripts/run_ardupilot.py --pattern square
python scripts/run_ardupilot.py --pattern circle
python scripts/run_ardupilot.py --pattern hover
```

### Options

| Option | Default | Description |
|--------|---------|-------------|
| `--pattern, -p` | square | Flight pattern: square, circle, hover |
| `--altitude, -a` | 5.0 | Flight altitude in meters |
| `--size, -s` | 5.0 | Pattern size (side length or radius) |
| `--connection, -c` | tcp:127.0.0.1:5760 | MAVLink connection string |
| `--gps` | false | Use GPS mode (default is GPS-denied) |

### Examples

```bash
# Large square at high altitude
python scripts/run_ardupilot.py --pattern square --size 10 --altitude 15

# Small circle
python scripts/run_ardupilot.py --pattern circle --size 3 --altitude 5

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

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

## Python API

### Basic Usage

```python
from src.drone_controller import DroneController

# Create and connect
drone = DroneController()
if not drone.connect():
    print("Connection failed")
    exit(1)

# Setup for GPS-denied (disables arming checks)
drone.setup_gps_denied()

# Set mode and arm
drone.set_mode("GUIDED_NOGPS")  # or "GUIDED" for GPS mode
drone.arm()

# Takeoff
drone.takeoff(5.0)

# Fly a pattern
drone.fly_square(size=5, altitude=5)

# Land
drone.land()
```

### DroneController Class

#### Constructor

```python
drone = DroneController(connection_string="tcp:127.0.0.1:5760")
```

#### Properties

| Property | Type | Description |
|----------|------|-------------|
| `armed` | bool | Is the drone armed? |
| `mode` | str | Current flight mode |
| `altitude` | float | Current altitude (meters) |
| `position` | dict | Current position {"x", "y", "z"} |
| `vehicle_type` | str | "copter" or "plane" |

#### Methods

| Method | Description |
|--------|-------------|
| `connect(timeout=30)` | Connect to SITL, returns True/False |
| `set_mode(mode)` | Set flight mode (GUIDED, GUIDED_NOGPS, LAND, etc.) |
| `setup_gps_denied()` | Configure for GPS-denied operation |
| `arm()` | Arm motors (force arm) |
| `takeoff(altitude)` | 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 until reached |
| `fly_square(size, alt)` | Fly square pattern |
| `fly_circle(radius, alt)` | Fly circle pattern |
| `land()` | Land the drone |
| `set_param(name, value)` | Set an ArduPilot parameter |

### Flight Modes (ArduCopter)

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

## Coordinate System

ArduPilot uses NED (North-East-Down):
- **X (North)**: Positive = forward
- **Y (East)**: Positive = right
- **Z (Down)**: Positive = down, **negative = up!**

```python
# Go 5m forward, 3m right, at 10m altitude
drone.goto(5, 3, -10)  # Z is negative for altitude!
```

## GPS-Denied Mode

The controller defaults to GPS-denied mode, which:

1. Disables arming checks (`ARMING_CHECK=0`)
2. Uses `GUIDED_NOGPS` mode (ID 20)
3. Relies on barometer for altitude

To use GPS mode instead:
```bash
python scripts/run_ardupilot.py --pattern square --gps
```

## Troubleshooting

### Drone won't arm

1. Check MAVProxy console for pre-arm errors
2. Try force arming in MAVProxy: `arm throttle force`
3. Make sure SITL is connected to Gazebo (shows "JSON received:")

### Mode not changing

Check the current mode in MAVProxy console. If stuck, try:
```
mode GUIDED
```

### Drone not moving

1. Make sure you're in GUIDED or GUIDED_NOGPS mode
2. Check altitude - drone needs to be off ground
3. Verify Gazebo is running and responsive

### Wrong vehicle type detected

If you see ArduPlane modes (FBWA, FBWB), restart SITL:
```bash
pkill -9 -f sim_vehicle
sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --console --wipe-eeprom
```