# DroneController Guide (GPS-Denied)

Implement your landing algorithm in `drone_controller.py`.

## Quick Start

1. Edit `drone_controller.py`
2. Find `calculate_landing_maneuver()`
3. Implement your algorithm
4. Test with any mode:
   - `python standalone_simulation.py --pattern stationary` (standalone)
   - `python run_bridge.py --pattern stationary` (PyBullet + ROS 2)
   - `python run_gazebo.py --pattern stationary` (Gazebo + ROS 2)

## GPS-Denied Challenge

No GPS available. You must use:

| Sensor | Data |
|--------|------|
| **IMU** | Orientation, angular velocity |
| **Altimeter** | Altitude, vertical velocity |
| **Velocity** | Estimated from optical flow |
| **Camera** | 320x240 downward image (base64 JPEG) |
| **Landing Pad** | Relative position (may be null!) |

## Function to Implement

```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
    # Your code here
    return (thrust, pitch, roll, yaw)
```

## Sensor Data

### IMU
```python
imu = telemetry['imu']
roll = imu['orientation']['roll']
pitch = imu['orientation']['pitch']
yaw = imu['orientation']['yaw']
angular_vel = imu['angular_velocity']  # {x, y, z}
```

### Altimeter
```python
altimeter = telemetry['altimeter']
altitude = altimeter['altitude']
vertical_vel = altimeter['vertical_velocity']
```

### Velocity
```python
velocity = telemetry['velocity']  # {x, y, z} in m/s
```

### Camera
The drone has a downward-facing camera providing 320x240 JPEG images.

```python
import base64
from PIL import Image
import io

camera = telemetry['camera']
image_b64 = camera.get('image')

if image_b64:
    image_bytes = base64.b64decode(image_b64)
    image = Image.open(io.BytesIO(image_bytes))
    # Process image for custom vision algorithms
```

### Landing Pad (Vision)
**Important: May be None if pad not visible!**

```python
landing_pad = telemetry['landing_pad']

if landing_pad is not None:
    relative_x = landing_pad['relative_x']   # body frame
    relative_y = landing_pad['relative_y']   # body frame
    distance = landing_pad['distance']       # vertical
    confidence = landing_pad['confidence']   # 0-1
```

## Control Output

| Value | Range | Effect |
|-------|-------|--------|
| thrust | ±1.0 | Up/down |
| pitch | ±0.5 | Forward/back |
| roll | ±0.5 | Left/right |
| yaw | ±0.5 | Rotation |

## Example Algorithm

```python
def calculate_landing_maneuver(self, telemetry, rover_telemetry):
    altimeter = telemetry.get('altimeter', {})
    altitude = altimeter.get('altitude', 5.0)
    vertical_vel = altimeter.get('vertical_velocity', 0.0)
    
    velocity = telemetry.get('velocity', {})
    vel_x = velocity.get('x', 0.0)
    vel_y = velocity.get('y', 0.0)
    
    landing_pad = telemetry.get('landing_pad')
    
    # Altitude control
    thrust = 0.5 * (0 - altitude) - 0.3 * vertical_vel
    
    # Horizontal control
    if landing_pad is not None:
        pitch = 0.3 * landing_pad['relative_x'] - 0.2 * vel_x
        roll = 0.3 * landing_pad['relative_y'] - 0.2 * vel_y
    else:
        pitch = -0.2 * vel_x
        roll = -0.2 * vel_y
    
    return (thrust, pitch, roll, 0.0)
```

## Using the Camera

You can implement custom vision processing on the camera image:

```python
import cv2
import numpy as np
import base64

def process_camera(telemetry):
    camera = telemetry.get('camera', {})
    image_b64 = camera.get('image')
    
    if not image_b64:
        return None
    
    # Decode JPEG
    image_bytes = base64.b64decode(image_b64)
    nparr = np.frombuffer(image_bytes, np.uint8)
    image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
    
    # Example: detect green landing pad
    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
    green_mask = cv2.inRange(hsv, (35, 50, 50), (85, 255, 255))
    
    # Find contours
    contours, _ = cv2.findContours(green_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
    
    if contours:
        largest = max(contours, key=cv2.contourArea)
        M = cv2.moments(largest)
        if M['m00'] > 0:
            cx = int(M['m10'] / M['m00'])
            cy = int(M['m01'] / M['m00'])
            # cx, cy is center of detected pad in image coordinates
            return (cx, cy)
    
    return None
```

## Strategies

### When Pad Not Visible
- Maintain altitude and stabilize
- Search by ascending or spiraling
- Dead reckoning from last known position

### State Machine
1. Search → find pad
2. Approach → move above pad
3. Align → center over pad
4. Descend → controlled descent
5. Land → touch down

## Testing

```bash
# Easy - stationary rover
python standalone_simulation.py --pattern stationary

# Medium - slow circular movement
python standalone_simulation.py --pattern circular --speed 0.2

# Hard - faster random movement
python standalone_simulation.py --pattern random --speed 0.3

# With ROS 2 (Gazebo)
ros2 launch gazebo/launch/drone_landing.launch.py  # Terminal 1
python run_gazebo.py --pattern circular            # Terminal 2
```

## Configuration

Edit `config.py` to tune controller gains:

```python
CONTROLLER = {
    "Kp_z": 0.5,    # Altitude proportional gain
    "Kd_z": 0.3,    # Altitude derivative gain
    "Kp_xy": 0.3,   # Horizontal proportional gain
    "Kd_xy": 0.2,   # Horizontal derivative gain
}
```