127 lines
2.9 KiB
Markdown
127 lines
2.9 KiB
Markdown
# DroneController Guide
|
|
|
|
Implement your GPS-denied landing algorithm in `drone_controller.py`.
|
|
|
|
## Quick Start
|
|
|
|
1. Edit `drone_controller.py`
|
|
2. Find `calculate_landing_maneuver()`
|
|
3. Implement your algorithm
|
|
4. Test: `python standalone_simulation.py`
|
|
|
|
## Sensors Available
|
|
|
|
| Sensor | Description |
|
|
|--------|-------------|
|
|
| IMU | Orientation, angular velocity |
|
|
| Altimeter | Altitude, vertical velocity |
|
|
| Velocity | Estimated velocity (x, y, z) |
|
|
| Camera | 320x240 downward image |
|
|
| 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
|
|
|
|
```python
|
|
# Altitude
|
|
altitude = telemetry['altimeter']['altitude']
|
|
vertical_vel = telemetry['altimeter']['vertical_velocity']
|
|
|
|
# Velocity
|
|
vel_x = telemetry['velocity']['x']
|
|
vel_y = telemetry['velocity']['y']
|
|
|
|
# Landing Pad (may be None!)
|
|
landing_pad = telemetry.get('landing_pad')
|
|
if landing_pad:
|
|
relative_x = landing_pad['relative_x']
|
|
relative_y = landing_pad['relative_y']
|
|
```
|
|
|
|
## 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:
|
|
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
|
|
|
|
```python
|
|
import cv2
|
|
import numpy as np
|
|
import base64
|
|
|
|
camera = telemetry.get('camera', {})
|
|
image_b64 = camera.get('image')
|
|
|
|
if image_b64:
|
|
image_bytes = base64.b64decode(image_b64)
|
|
nparr = np.frombuffer(image_bytes, np.uint8)
|
|
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
|
# Process image...
|
|
```
|
|
|
|
## Testing
|
|
|
|
```bash
|
|
# Easy - stationary
|
|
python standalone_simulation.py --pattern stationary
|
|
|
|
# Medium - circular
|
|
python standalone_simulation.py --pattern circular --speed 0.3
|
|
|
|
# Hard - random
|
|
python standalone_simulation.py --pattern random --speed 0.5
|
|
```
|
|
|
|
## Configuration
|
|
|
|
Edit `config.py`:
|
|
|
|
```python
|
|
CONTROLLER = {
|
|
"Kp_z": 0.5, # Altitude proportional
|
|
"Kd_z": 0.3, # Altitude derivative
|
|
"Kp_xy": 0.3, # Horizontal proportional
|
|
"Kd_xy": 0.2, # Horizontal derivative
|
|
}
|
|
```
|