# Drone Controller Guide

## Overview

The drone controller (`src/drone_controller.py`) provides a simple interface for flying the drone in simulation.

## Usage

### Command Line

```bash
source ~/venv-ardupilot/bin/activate
cd ~/RDC_Simulation

# Fly a square pattern
python scripts/run_ardupilot.py --pattern square

# Options
python scripts/run_ardupilot.py --help
```

### Options

| Option | Default | Description |
|--------|---------|-------------|
| `--pattern` | square | Flight pattern: square, circle, hover |
| `--altitude` | 5 | Flight altitude in meters |
| `--size` | 5 | Pattern size (side length or radius) |
| `--connection` | tcp:127.0.0.1:5760 | MAVLink connection string |

### Examples

```bash
# Fly a 10m square at 8m altitude
python scripts/run_ardupilot.py --pattern square --size 10 --altitude 8

# Fly a circle with 5m radius
python scripts/run_ardupilot.py --pattern circle --size 5

# Just hover (useful for testing)
python scripts/run_ardupilot.py --pattern hover
```

## 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)

# Takeoff
drone.set_mode("GUIDED")
drone.arm()
drone.takeoff(5.0)

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

# Land
drone.land()
```

### Class: DroneController

#### Connection

```python
drone = DroneController(connection_string="tcp:127.0.0.1:5760")
drone.connect(timeout=30)  # Returns True/False
```

#### State

```python
drone.armed      # bool: Is the drone armed?
drone.mode       # str: Current flight mode
drone.altitude   # float: Current altitude (m)
drone.position   # dict: {"x": float, "y": float, "z": float}
```

#### Commands

```python
drone.set_mode("GUIDED")       # Set flight mode
drone.arm()                    # Arm motors (force arm)
drone.takeoff(5.0)             # Takeoff to altitude
drone.goto(x, y, z)            # Go to position (NED frame)
drone.fly_to_and_wait(x, y, alt)  # Go and wait until reached
drone.land()                   # Land
```

#### Patterns

```python
drone.fly_square(size=5, altitude=5)    # Square pattern
drone.fly_circle(radius=5, altitude=5)  # Circle pattern
```

## Coordinate System

ArduPilot uses NED (North-East-Down):
- **X**: North (positive forward)
- **Y**: East (positive right)
- **Z**: Down (negative is up!)

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

## Flight Modes

| Mode | Description |
|------|-------------|
| GUIDED | Accept external commands |
| LAND | Automatic landing |
| LOITER | Hold position |
| RTL | Return to launch |
| STABILIZE | Manual control |

## Sequence Diagram

```
┌─────────┐      ┌──────────┐      ┌─────────┐
│ Gazebo  │      │   SITL   │      │ Script  │
└────┬────┘      └────┬─────┘      └────┬────┘
     │                │                  │
     │ Physics data   │                  │
     │ ◄──────────────│                  │
     │                │                  │
     │ Sensor JSON    │                  │
     │ ───────────────►                  │
     │                │   Connect        │
     │                │ ◄────────────────│
     │                │                  │
     │                │   Set GUIDED     │
     │                │ ◄────────────────│
     │                │                  │
     │                │   Arm            │
     │                │ ◄────────────────│
     │                │                  │
     │                │   Takeoff        │
     │                │ ◄────────────────│
     │                │                  │
     │ Motor commands │                  │
     │ ◄──────────────│                  │
     │                │                  │
     │ Drone moves    │                  │
     │                │                  │
```

## Troubleshooting

### Drone doesn't move

1. Check Gazebo is running
2. Check SITL shows telemetry (not "No JSON sensor message")
3. Check drone is armed

### Timeout waiting for altitude

- Increase takeoff timeout
- Check for pre-arm failures in SITL console

### Connection refused

```bash
# Check SITL is running
nc -z 127.0.0.1 5760 && echo "SITL is ready" || echo "SITL not running"
```