This page presents key implementation snippets from our cooperative drone system. All code is written in Python and demonstrates the core algorithms discussed in the technical documentation.
Implementation Language: Python 3.8+
Key Libraries: OpenCV, Ultralytics (YOLOv8), NumPy, djitellopy, pyardrone
Project Code: Github
File: Tello/obstacle_avoidance/obstacle_detector.py
"""
Obstacle detection and dimension estimation using YOLOv8.
"""
import cv2
import numpy as np
from typing import List, Dict
from ultralytics import YOLO
class ObstacleDetector:
"""Real-time object detection and distance estimation"""
def __init__(self, config: Config = Config()):
"""
Initialize obstacle detector with YOLOv8 model.
Args:
config: Configuration object with detection parameters
"""
self.config = config
self.frame_count = 0
# Load YOLOv8 nano model (will download on first run)
print(f"[*] Loading YOLO model: {config.YOLO_MODEL}")
self.model = YOLO(config.YOLO_MODEL) # "yolov8n.pt"
print("[+] YOLO model loaded successfully")
self.focal_length = config.FOCAL_LENGTH_PX # 700 pixels
self.object_heights = config.OBJECT_HEIGHTS # Dictionary of known heights
yolov8n.pt) for real-time performancedef detect_objects(self, frame: np.ndarray) -> List[Dict]:
"""
Run YOLO detection on frame.
Args:
frame: Input image (BGR format from OpenCV)
Returns:
List of detected objects with bounding boxes and confidence
"""
# Run YOLOv8 inference
results = self.model(frame, conf=self.config.DETECTION_CONFIDENCE, verbose=False)
detections = []
for result in results:
boxes = result.boxes
for box in boxes:
# Extract box data
x1, y1, x2, y2 = box.xyxy[0].cpu().numpy()
confidence = float(box.conf[0])
class_id = int(box.cls[0])
class_name = self.model.names[class_id]
detection = {
"bbox": {
"x": int(x1),
"y": int(y1),
"w": int(x2 - x1),
"h": int(y2 - y1),
},
"class": class_name,
"confidence": confidence,
"class_id": class_id
}
detections.append(detection)
return detections
def estimate_distance(self, bbox_height: int, object_class: str) -> float:
"""
Estimate distance to object using pinhole camera model.
Formula: distance = (known_height * focal_length) / pixel_height
Args:
bbox_height: Height of bounding box in pixels
object_class: Class name of detected object
Returns:
Estimated distance in meters
"""
if bbox_height == 0:
return 999.0 # Very far away
# Get known height for this object class
# Example: person = 1.7m, chair = 0.9m, table = 0.75m
known_height = self.object_heights.get(
object_class,
self.object_heights["generic"] # Default: 1.0m
)
# Calculate distance using pinhole camera model
# d = (H × f) / h
distance_m = (known_height * self.focal_length) / bbox_height
return distance_m
$$d = \frac{H \times f}{h}$$
Where:
File: AR Drone/aruco_detection/detector.py
"""
ARuco Marker Detector for AR Drone
Uses OpenCV's ArUco module for marker detection and pose estimation.
"""
import cv2
import cv2.aruco as aruco
import numpy as np
class ArucoDetector:
"""ARuco marker detector using OpenCV."""
def __init__(self, dictionary=aruco.DICT_4X4_50, target_id=1):
"""
Initialize ARuco detector.
Args:
dictionary: ARuco dictionary type (default: DICT_4X4_50)
target_id: Target ARuco marker ID to search for
"""
self.target_id = target_id
self.aruco_dict = aruco.getPredefinedDictionary(dictionary)
self.aruco_params = aruco.DetectorParameters()
print(f"[ARUCO] Initialized detector for ID {target_id}")
print(f"[ARUCO] Dictionary: {dictionary}")
def detect(self, frame):
"""
Detect ARuco markers in frame.
Args:
frame: OpenCV BGR image frame
Returns:
list of dict with marker information
"""
# Convert to grayscale for better detection
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect markers using OpenCV ArUco detector
corners, ids, rejected = aruco.detectMarkers(
gray, self.aruco_dict, parameters=self.aruco_params
)
markers = []
if ids is not None:
for i, marker_id in enumerate(ids.flatten()):
marker_corners = corners[i][0]
# Calculate center of marker
cx = int(np.mean(marker_corners[:, 0]))
cy = int(np.mean(marker_corners[:, 1]))
# Calculate marker area using cross product
x = marker_corners[:, 0]
y = marker_corners[:, 1]
area = 0.5 * abs(
(x[0]*y[1] - x[1]*y[0]) +
(x[1]*y[2] - x[2]*y[1]) +
(x[2]*y[3] - x[3]*y[2]) +
(x[3]*y[0] - x[0]*y[3])
)
markers.append({
'id': int(marker_id),
'corners': marker_corners,
'center': (cx, cy),
'area': area
})
return markers
File: Tello/pid_controller/position_controller.py
"""
PID Controller for precise drone position control.
Implements the classic PID control law:
u(t) = Kp*e(t) + Ki*∫e(τ)dτ + Kd*de(t)/dt
"""
import time
class PIDController:
"""Single-axis PID controller with anti-windup."""
def __init__(self, kp: float, ki: float, kd: float,
integral_limit: float = 50.0, deadband: float = 0.0):
"""
Initialize PID controller.
Args:
kp: Proportional gain
ki: Integral gain
kd: Derivative gain
integral_limit: Maximum integral term (anti-windup)
deadband: Error threshold below which output is zero
"""
self.kp = kp
self.ki = ki
self.kd = kd
self.integral_limit = integral_limit
self.deadband = deadband
# State variables
self.integral = 0.0
self.previous_error = 0.0
self.previous_time = None
def compute(self, error: float, current_time: float = None) -> float:
"""
Compute PID control output.
Args:
error: Current error (target - current)
current_time: Current timestamp (uses time.time() if None)
Returns:
Control output u(t)
"""
# Get current time if not provided
if current_time is None:
current_time = time.time()
# Calculate time delta
if self.previous_time is None:
dt = 0.0
else:
dt = current_time - self.previous_time
# Apply deadband to ignore small errors
if abs(error) < self.deadband:
error = 0.0
self.integral = 0.0 # Reset integral in deadband
# Proportional term: P = Kp × e(t)
p_term = self.kp * error
# Integral term with anti-windup: I = Ki × ∫e(τ)dτ
if dt > 0:
self.integral += error * dt
# Clamp integral to prevent windup
self.integral = max(-self.integral_limit,
min(self.integral, self.integral_limit))
i_term = self.ki * self.integral
# Derivative term: D = Kd × de(t)/dt
if dt > 0:
derivative = (error - self.previous_error) / dt
else:
derivative = 0.0
d_term = self.kd * derivative
# Update state for next iteration
self.previous_error = error
self.previous_time = current_time
# Compute total output: u(t) = P + I + D
output = p_term + i_term + d_term
return output
$$u(t) = K_p \cdot e(t) + K_i \cdot \int_{0}^{t} e(\tau) d\tau + K_d \cdot \frac{de(t)}{dt}$$
File: Tello/obstacle_avoidance/path_planner.py
def bresenham_line(x0: int, y0: int, x1: int, y1: int) -> List[Tuple[int, int]]:
"""
Generate all integer points on a line using Bresenham's Line Algorithm.
Classic algorithm for drawing lines on a pixel grid using only
integer arithmetic (no floating-point operations).
Time Complexity: O(max(dx, dy))
Space Complexity: O(max(dx, dy))
Args:
x0, y0: Start point coordinates (integers)
x1, y1: End point coordinates (integers)
Returns:
List of (x, y) integer coordinate tuples
Example:
>>> bresenham_line(0, 0, 5, 3)
[(0,0), (1,1), (2,1), (3,2), (4,2), (5,3)]
"""
points = []
# Calculate deltas
dx = abs(x1 - x0)
dy = abs(y1 - y0)
# Determine step direction
sx = 1 if x0 < x1 else -1
sy = 1 if y0 < y1 else -1
# Initialize error accumulator
err = dx - dy
# Current position
x, y = x0, y0
while True:
# Add current point to path
points.append((x, y))
# Check if we've reached the end
if x == x1 and y == y1:
break
# Calculate error * 2 (avoids floating point)
e2 = 2 * err
# Step in x direction if needed
if e2 > -dy:
err -= dy
x += sx
# Step in y direction if needed
if e2 < dx:
err += dx
y += sy
return points
def generate_waypoints_linear(start_x: float, start_y: float,
goal_x: float, goal_y: float,
altitude: float,
num_intermediate: int = 3) -> List[Tuple[float, float, float]]:
"""
Generate waypoints using linear interpolation.
Creates evenly-spaced waypoints along a straight line from start to goal.
Obstacle avoidance is handled reactively during flight.
Args:
start_x, start_y: Starting position (meters)
goal_x, goal_y: Goal position (meters)
altitude: Flight altitude (meters)
num_intermediate: Number of intermediate waypoints
Returns:
List of (x, y, z) waypoint tuples
"""
waypoints = []
# Add start waypoint
waypoints.append((start_x, start_y, altitude))
# Generate intermediate waypoints
for i in range(1, num_intermediate + 1):
t = i / (num_intermediate + 1) # Interpolation parameter [0, 1]
# Linear interpolation: p(t) = (1-t)·p0 + t·p1
x = start_x + t * (goal_x - start_x)
y = start_y + t * (goal_y - start_y)
waypoints.append((x, y, altitude))
# Add goal waypoint
waypoints.append((goal_x, goal_y, altitude))
# Add landing waypoint (z = 0)
waypoints.append((goal_x, goal_y, 0))
return waypoints
The Tello Drone uses a hybrid approach combining:
File: AR Drone/path_planning/grid_search.py
def _generate_snake_pattern(self):
"""
Generate snake/boustrophedon search pattern.
Moves left-to-right on even rows, right-to-left on odd rows.
Example for 5×5 grid:
Row 0: → → → → →
Row 1: ← ← ← ← ←
Row 2: → → → → →
Row 3: ← ← ← ← ←
Row 4: → → → → →
"""
waypoints = []
for row in range(self.grid_size):
if row % 2 == 0:
# Left to right on even rows
for col in range(self.grid_size):
waypoints.append((row, col))
else:
# Right to left on odd rows
for col in range(self.grid_size - 1, -1, -1):
waypoints.append((row, col))
return waypoints
def update_position_dead_reckoning(self, vx: float, vy: float,
yaw_deg: float, dt: float):
"""
Update estimated position using dead reckoning.
Integrates velocity commands with IMU yaw to estimate position
in world frame. Accumulates error over time (±10-30cm per 3m).
Args:
vx: Forward velocity (m/s) in body frame
vy: Right velocity (m/s) in body frame
yaw_deg: Current yaw angle (degrees) from IMU
dt: Time step (seconds)
"""
# Convert yaw to radians
yaw_rad = np.deg2rad(yaw_deg)
# Rotation matrix: Body frame → World frame
# [x_world] [cos(ψ) -sin(ψ)] [vx]
# [y_world] = [sin(ψ) cos(ψ)] [vy]
# Transform velocities to world frame
vx_world = vx * np.cos(yaw_rad) - vy * np.sin(yaw_rad)
vy_world = vx * np.sin(yaw_rad) + vy * np.cos(yaw_rad)
# Integrate position: x' = x + v × Δt
self.estimated_x += vx_world * dt
self.estimated_y += vy_world * dt
# Update yaw
self.estimated_yaw = yaw_deg
return (self.estimated_x, self.estimated_y, self.estimated_yaw)
Position update in world frame:
$$x' = x + (v_x \cos\psi - v_y \sin\psi) \cdot \Delta t$$ $$y' = y + (v_x \sin\psi + v_y \cos\psi) \cdot \Delta t$$
Where:
File: Tello/obstacle_avoidance/config.py
class Config:
"""Configuration parameters for Tello obstacle avoidance system."""
# YOLOv8 Model
YOLO_MODEL = "yolov8n.pt" # Nano model for speed
DETECTION_CONFIDENCE = 0.5 # Minimum confidence threshold
# Camera Calibration
FOCAL_LENGTH_PX = 700 # Empirically calibrated
IMAGE_WIDTH = 960
IMAGE_HEIGHT = 720
# Known Object Heights (meters)
OBJECT_HEIGHTS = {
"person": 1.7,
"chair": 0.9,
"dining table": 0.75,
"couch": 0.85,
"bed": 0.6,
"generic": 1.0 # Default for unknown objects
}
# Threat Assessment Thresholds (meters)
THREAT_HIGH_DISTANCE = 1.5 # Immediate action required
THREAT_MEDIUM_DISTANCE = 2.5 # Cautious navigation
# Obstacle Avoidance Maneuvers (cm)
DODGE_LATERAL_CM = 80 # Side dodge distance
DODGE_VERTICAL_CM = 60 # Vertical dodge distance
# PID Controller Gains
PID_KP = 1.0 # Proportional gain
PID_KI = 0.1 # Integral gain
PID_KD = 0.3 # Derivative gain
# Flight Parameters
CRUISE_SPEED_CM_S = 50 # Normal flight speed
PRECISION_SPEED_CM_S = 20 # Slow speed for landing
FLIGHT_ALTITUDE_CM = 100 # Default cruise altitude
EECSc106aFinal/
├── Tello/ # Tello Drone (Parent/Payload)
│ ├── obstacle_avoidance/
│ │ ├── obstacle_detector.py # YOLOv8 detection + distance estimation
│ │ ├── path_planner.py # Bresenham + linear interpolation
│ │ ├── circumvent.py # Dodge maneuvers
│ │ └── config.py # Configuration parameters
│ ├── pid_controller/
│ │ └── position_controller.py # PID implementation
│ ├── visualization/
│ │ ├── live_map.py # Real-time trajectory display
│ │ └── web_dashboard.py # Web-based monitoring
│ └── navigate.py # Main navigation loop
│
├── AR Drone/ # AR Drone (Helper/Scout)
│ ├── aruco_detection/
│ │ ├── detector.py # ArUco marker detection
│ │ └── transform.py # Pose estimation
│ ├── path_planning/
│ │ ├── grid_search.py # Snake pattern grid search
│ │ └── visualizer.py # Path visualization
│ ├── pid_controller/
│ │ └── pid.py # PID for optical centering
│ └── main.py # Main execution script
│
└── website/ # Project documentation website
├── index.html # Home page
├── technical.html # Technical details
├── design-choices.html # Design decisions
└── code-snippets.html # This page
Continue exploring the project: