This document provides a comprehensive reference for the MAVSDK Drone Show control modes, coordinate systems, and origin handling strategies. It is intended for AI agents, developers, and operators who need to understand the precise behavior of different control configurations.
Standard aerospace coordinate frame used throughout the system:
All CSV trajectory files use NED coordinates. Altitude is negative (e.g., z=-30.0 means 30 meters above origin).
Animation software coordinate frame (converted to NED during processing):
Conversion to NED:
# X (north) => X (north) : unchanged
# Y (west) => Y (east) : multiply by -1
# Z (up) => Z (down) : multiply by -1
df['y [m]'] = -df['y [m]']
df['z [m]'] = -df['z [m]']
Global positioning using:
PX4’s local frame relative to arming position:
Fixed GPS reference point provided by GCS for swarm coordination:
Position setpoints in local NED frame:
auto_launch_position=True)Example:
# Drone armed at GPS (35.123456, -120.654321, 100m)
# CSV first row: (0.0, -5.0, 2.5, -5.0)
# Extracted launch: NED(-5.0, 2.5, -5.0) from arming position
# All waypoints adjusted relative to this launch position
Position setpoints in GPS LLA:
auto_launch_position=True)Example:
# Drone armed at GPS (35.123456, -120.654321, 100m)
# CSV first row: (0.0, -5.0, 2.5, -5.0)
# Waypoints converted to GPS using arming position as origin
# Each drone uses its own arming position as origin
Position setpoints in GPS LLA with shared origin:
initial_x=0.0, initial_y=0.0)Key Difference: In Phase 2, waypoints are absolute NED offsets from shared origin, not adjusted per drone. The shared origin is the reference point for the entire swarm formation.
Example:
# Shared origin: GPS (35.123456, -120.654321, 100m)
# Drone 1 placed at: GPS (35.123400, -120.654350, 100m) [~5m south, ~2.5m east]
# CSV first row: (0.0, -5.0, 2.5, -5.0)
# Phase 2 behavior:
# 1. Initial climb: BODY_VELOCITY (vx=0, vy=0, vz=-1.0)
# 2. After 5s/5m: Blend from current GPS to target GPS
# - Current: GPS (35.123400, -120.654350, 105m)
# - Target: shared_origin + CSV_waypoint[-5.0, 2.5, -5.0]
# - Target GPS: (35.123411, -120.654296, 95m)
# 3. 3-second interpolation: current_gps -> target_gps
# 4. Continue with GPS setpoints from shared origin
Allow operators to place drones with ±5-10 meter tolerance without compromising formation precision. The system automatically corrects position errors during mission start.
if effective_auto_origin_mode:
# Phase 2: NO adjustment (absolute waypoints)
waypoints = read_trajectory_file(
filename=trajectory_filename,
auto_launch_position=False,
initial_x=0.0, # Pass zeros to prevent subtraction
initial_y=0.0
)
else:
# Manual modes: Use config offsets or auto_launch
waypoints = read_trajectory_file(
filename=trajectory_filename,
auto_launch_position=effective_auto_launch,
initial_x=drone_config.initial_x,
initial_y=drone_config.initial_y
)
Why zeros in Phase 2?
Passing initial_x=0.0, initial_y=0.0 prevents adjust_waypoints() from subtracting offsets. This ensures CSV waypoints remain as absolute NED offsets from the shared origin.
Phase 2 forces BODY_VELOCITY mode:
# Always use body velocity in Phase 2
if effective_auto_origin_mode:
logger.info("Phase 2: Forcing BODY_VELOCITY initial climb mode")
initial_climb_mode = "BODY_VELOCITY"
Body velocity setpoints:
vx_body = 0.0 # No forward/back movement
vy_body = 0.0 # No left/right movement
vz_climb = -1.0 # Climb at 1 m/s (DOWN=-1 means UP)
yaw_deg = 0.0 # Face north
Why BODY_VELOCITY?
# Keep waypoint_index advancing for swarm synchronization
# Setpoints overridden with climb commands, but timeline continues
waypoint_index += 1
continue
Critical for synchronization:
# PHASE 2 FIX: Use CURRENT waypoint as blend target
# waypoint_index has advanced during climb to maintain timeline sync
current_waypoint = waypoints[waypoint_index]
(t_wp_0, px_0, py_0, pz_0, vx_0, vy_0, vz_0, ...) = current_waypoint
Why current waypoint? After 5 seconds of climb, the timeline is at ~5 seconds. The CSV waypoint at t=5s might be 30m altitude (drone climbed from 0→30m in show). Using waypoints[0] (t=0s, z=0m) would pull drone back down.
# Get current GPS position
current_lla = drone_lla
# Target GPS from shared origin + CSV NED
target_lla = convert_ned_to_lla(
north=px_0,
east=py_0,
down=pz_0,
origin_lat=origin_lat_deg,
origin_lon=origin_lon_deg,
origin_alt=origin_alt_m
)
# Interpolate over 3 seconds
for blend_t in range(0, 3000, 50): # 50ms steps
alpha = blend_t / 3000.0 # 0.0 -> 1.0
blended_lat = current_lla.lat + alpha * (target_lla.lat - current_lla.lat)
blended_lon = current_lla.lon + alpha * (target_lla.lon - current_lla.lon)
blended_alt = current_lla.alt + alpha * (target_lla.alt - current_lla.alt)
await drone.offboard.set_position_global(
PositionGlobalYaw(blended_lat, blended_lon, blended_alt, yaw_deg)
)
Why 3 seconds?
Key Principles:
Provide safe vertical separation from ground before starting horizontal maneuvers. Ensures drones clear obstacles and reach stable altitude.
actual_alt = -pz # Current waypoint altitude
under_alt = actual_alt < Params.INITIAL_CLIMB_ALTITUDE_THRESHOLD # Default: 5.0m
under_time = time_in_climb < Params.INITIAL_CLIMB_TIME_THRESHOLD # Default: 5.0s
in_initial_climb = under_alt or under_time # BOTH must be satisfied
Climb continues until:
Why dual check?
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(
forward_m_s=0.0, # vx_body
right_m_s=0.0, # vy_body
down_m_s=-1.0, # vz_climb (negative = up)
yawspeed_deg_s=0.0
)
)
Characteristics:
await drone.offboard.set_velocity_ned(
VelocityNedYaw(
north_m_s=0.0, # vx_climb
east_m_s=0.0, # vy_climb
down_m_s=-1.0, # vz_climb
yaw_deg=yaw_deg
)
)
Characteristics:
Problem (Fixed in commit feaee038):
# WRONG: CSV vz values contain numerical noise
vz_climb = vz if abs(vz) > 1e-6 else Params.INITIAL_CLIMB_VZ_DEFAULT
# CSV had vz=0.0026 -> treated as valid -> drone didn't climb
Solution:
# CORRECT: Always use configured climb speed
vz_climb = Params.INITIAL_CLIMB_VZ_DEFAULT # Unconditional
Why this happened:
Problem (Fixed in commit feaee038):
# WRONG: Timeline frozen during climb
await asyncio.sleep(0.05)
continue # Skips waypoint_index increment
Solution:
# CORRECT: Timeline advances during climb
waypoint_index += 1
continue # Increments before continuing
Why this matters:
Precise time synchronization is critical for swarm formations. All drones must execute waypoints at exactly the same timeline position, regardless of performance variations.
Drift calculation:
elapsed = time.time() - mission_start_time # Actual time elapsed
drift_delta = elapsed - t_wp # Drift from timeline
Drift states:
drift_delta > 0: Behind schedule (need to catch up)drift_delta < 0: Ahead of schedule (need to wait)drift_delta = 0: On time (perfect sync)if drift_delta >= 0:
# Limit catchup to prevent excessive skipping
safe_drift = min(drift_delta, Params.DRIFT_CATCHUP_MAX_SEC) # Default: 0.5s
# Calculate skip count (limited to prevent formation breaks)
skip_count = int(safe_drift / csv_step)
skip_count = min(skip_count, Params.MAX_WAYPOINT_SKIP_PER_ITERATION) # Max: 5
if skip_count > 0:
logger.warning(
f"⚠️ DRIFT CATCHUP: Skipping {skip_count} waypoints "
f"(drift={drift_delta:.2f}s, from WP{waypoint_index} "
f"to WP{waypoint_index+skip_count})"
)
waypoint_index = min(waypoint_index + skip_count, total_waypoints - 1)
drift_stats['skip_events'] += 1
drift_stats['total_waypoints_skipped'] += skip_count
Key parameters:
Why limit skipping?
else: # drift_delta < 0
sleep_duration = t_wp - elapsed # How much ahead
if sleep_duration > 0:
await asyncio.sleep(min(sleep_duration, Params.AHEAD_SLEEP_STEP_SEC))
Key parameters:
Why limit sleep?
# Detect severe drift (production alert)
if drift_delta > Params.SEVERE_DRIFT_THRESHOLD:
logger.error(
f"🚨 SEVERE DRIFT: {drift_delta:.2f}s behind timeline "
f"(threshold={Params.SEVERE_DRIFT_THRESHOLD}s) - "
f"Performance insufficient for real-time execution!"
)
drift_stats['severe_drift_events'] += 1
Key parameters:
Why alert?
drift_stats = {
'max_drift_behind': 0.0,
'max_drift_ahead': 0.0,
'skip_events': 0,
'total_waypoints_skipped': 0,
'severe_drift_events': 0,
'ahead_wait_events': 0
}
# Update during mission
if drift_delta > 0:
drift_stats['max_drift_behind'] = max(drift_stats['max_drift_behind'], drift_delta)
else:
drift_stats['max_drift_ahead'] = max(drift_stats['max_drift_ahead'], abs(drift_delta))
# Log at end
logger.info(
f"📊 DRIFT STATISTICS:\n"
f" Max Behind: {drift_stats['max_drift_behind']:.3f}s\n"
f" Max Ahead: {drift_stats['max_drift_ahead']:.3f}s\n"
f" Skip Events: {drift_stats['skip_events']}\n"
f" Waypoints Skipped: {drift_stats['total_waypoints_skipped']}\n"
f" Severe Drift Events: {drift_stats['severe_drift_events']}\n"
f" Ahead Wait Events: {drift_stats['ahead_wait_events']}"
)
Benefits:
# After initial climb, verify timeline synchronization
if just_finished_climb:
expected_index = int(time_in_climb / csv_step)
actual_index = waypoint_index
sync_error = abs(actual_index - expected_index)
if sync_error > 10: # More than 0.1s error
logger.warning(
f"⚠️ TIMELINE SYNC WARNING: After initial climb, "
f"waypoint_index={actual_index} but expected ~{expected_index} "
f"(error={sync_error} waypoints = {sync_error*csv_step:.2f}s)"
)
else:
logger.info(
f"✅ TIMELINE SYNC VERIFIED: waypoint_index={actual_index}, "
f"expected={expected_index} (error={sync_error} waypoints)"
)
Purpose:
The launch position (where the drone starts in the formation) can be determined from two sources:
Different control modes use different sources.
if not effective_use_global_setpoints:
# LOCAL mode: Extract from CSV first row
effective_auto_launch = True
logger.info("LOCAL mode: Extracting launch position from trajectory CSV first row")
elif effective_auto_origin_mode:
# GLOBAL Phase 2: NO adjustment (absolute waypoints)
effective_auto_launch = False
logger.info("GLOBAL Phase 2 mode: Using absolute waypoints, no adjustment")
else:
# GLOBAL manual: Extract from CSV first row
effective_auto_launch = True
logger.info("GLOBAL manual: Extracting launch position from trajectory CSV first row")
| Mode | Source | auto_launch_position | initial_x/y |
|---|---|---|---|
| LOCAL | CSV first row | True | Ignored |
| GLOBAL Manual | CSV first row | True | Ignored |
| GLOBAL Phase 2 | Telemetry (current GPS) | False | 0.0, 0.0 |
Problem with config.csv values:
Example scenario:
# config.csv (old values from previous show)
drone_config.initial_x = 10.0 # Old position
drone_config.initial_y = 5.0
# Current show CSV first row
# t, px, py, pz
# 0.0, -5.0, 2.5, 0.0 # New formation position
# OLD BEHAVIOR (WRONG):
# Used initial_x=10.0, initial_y=5.0 from config.csv
# Subtracted from waypoints: (-5.0-10.0, 2.5-5.0) = (-15.0, -2.5)
# Drone went to wrong position!
# NEW BEHAVIOR (CORRECT):
# Extracted (-5.0, 2.5) from CSV first row
# Subtracted from waypoints: (-5.0-(-5.0), 2.5-2.5) = (0.0, 0.0)
# Drone stays at current position (correct launch)
Problem (Fixed in commit 84f0ae75):
# WRONG: Using config.csv values for manual modes
effective_auto_launch = False # Uses drone_config.initial_x/y
Solution:
# CORRECT: Extract from CSV first row for manual modes
effective_auto_launch = True # Extracts from CSV first row
Why this matters:
Likely cause: Phase 2 waypoint adjustment bug
Check:
# In drone_show.py, around line 2022-2037
if effective_auto_origin_mode:
# Should pass zeros to prevent adjustment
initial_x=0.0,
initial_y=0.0
Fix: Ensure Phase 2 passes (0.0, 0.0) to read_trajectory_file()
Likely cause: CSV vz value used instead of climb speed
Check:
# In drone_show.py, around line 749-751
# Should be unconditional
vz_climb = Params.INITIAL_CLIMB_VZ_DEFAULT
Fix: Always use configured climb speed, ignore CSV vz during initial climb
Likely cause: Timeline frozen during climb
Check:
# In drone_show.py, around line 763-766
# Should increment before continue
waypoint_index += 1
continue
Fix: Ensure waypoint_index advances during climb to maintain timeline sync
Likely cause: Using config.csv values instead of CSV first row
Check:
# In drone_show.py, around line 2015-2027
if not effective_use_global_setpoints:
effective_auto_launch = True # Extract from CSV
Fix: Use auto_launch_position=True for LOCAL and GLOBAL manual modes
Likely cause: Hardware insufficient for real-time execution
Check:
Fix:
docs/features/origin-system.mddocs/sitl_demo_docker.mdREADME.mdInitial Climb:
INITIAL_CLIMB_ALTITUDE_THRESHOLD = 5.0 (meters)INITIAL_CLIMB_TIME_THRESHOLD = 5.0 (seconds)INITIAL_CLIMB_VZ_DEFAULT = -1.0 (m/s, negative = up)Time Synchronization:
DRIFT_CATCHUP_MAX_SEC = 0.5 (maximum drift to catch up)MAX_WAYPOINT_SKIP_PER_ITERATION = 5 (maximum waypoints to skip)SEVERE_DRIFT_THRESHOLD = 2.0 (alert threshold)AHEAD_SLEEP_STEP_SEC = 0.05 (maximum sleep duration)Coordinate Conversion:
EARTH_RADIUS = 6371000.0 (meters)This document is maintained as part of the MAVSDK Drone Show project. For questions or updates, refer to the project repository.