Document Version: 1.0 Date: November 3, 2025 Status: Phase 1 Complete - Backend & Frontend Implemented Next Phase: Integration with drone_show.py execution loop
A comprehensive origin coordinate system was implemented for a professional drone show project, fixing critical coordinate bugs and adding altitude support. The system allows operators to:
OriginModal.js lines 128-129: Coordinates were swapped - was sending intended_east=drone.x, intended_north=drone.y. Now correctly sends intended_north=drone.x, intended_east=drone.y to match the config.csv schema where x=North, y=East (NED coordinate system).
The current drone show execution (drone_show.py) assumes drones are perfectly placed by the operator at their intended launch positions. If there are:
The show can go wrong because the execution loop zeros out CSV offsets from the assumed-perfect initial position.
Phase 2 Goal: Integrate this origin system into drone_show.py to enable a new mode where drones use precise global corrections after initial climb, ensuring the formation executes exactly as designed regardless of initial placement accuracy.
hw_id,pos_id,x,y,ip,mavlink_port,debug_port,gcs_ip
1,1,10.5,5.2,192.168.1.101,14551,13541,192.168.1.1
Definitive Mapping:
x column = North (meters)y column = East (meters)const n = parseFloat(drone.x); // North
const e = parseFloat(drone.y); // East
initial_x = float(row["x"]) # North
initial_y = float(row["y"]) # East
There are THREE distinct coordinate systems in this project:
config.csvdrone.telemetry.get_gps_global_origin()gcs-server/origin.jsonPX4 GPS Origin ≠ Formation Origin ≠ Launch Position
PX4 GPS Origin: Where PX4 thinks (0,0,0) is (auto-set at first GPS lock)
Formation Origin: Where WE want (0,0,0) to be (manually set)
Launch Position: Where a specific drone should physically be (calculated)
Formation Space (config.csv)
x=North, y=East (meters from formation origin)
↓
Apply pymap3d.ned2geodetic()
↓
GPS Coordinates (WGS84)
latitude, longitude, altitude MSL
↓
Send to drone as setpoint
↓
Offboard Mode Execution
(Local NED or Global LLA, configurable)
File: gcs-server/origin.py
Changes:
calculate_position_deviations() lines 92-93Before:
# WARNING: These comments were backwards!
config_x = float(drone.get('x', 0)) # x is East ❌ WRONG COMMENT
config_y = float(drone.get('y', 0)) # y is North ❌ WRONG COMMENT
After:
# Corrected comments
config_north = float(drone.get('x', 0)) # x is North ✅
config_east = float(drone.get('y', 0)) # y is East ✅
File: app/dashboard/drone-dashboard/src/components/OriginModal.js
Critical Bug Fix (lines 128-129, 137-138):
// BEFORE (BUG):
intended_east: parseFloat(drone.x) || 0, // ❌ WRONG
intended_north: parseFloat(drone.y) || 0, // ❌ WRONG
// AFTER (FIXED):
intended_north: parseFloat(drone.x) || 0, // ✅ x is North
intended_east: parseFloat(drone.y) || 0, // ✅ y is East
File: gcs-server/origin.py
Schema Evolution:
v1 (Old):
{
"lat": 37.7749,
"lon": -122.4194
}
v2 (New):
{
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"alt_source": "drone_telemetry",
"timestamp": "2025-11-03T10:30:45.123456",
"version": 2
}
Auto-Migration: Old origin.json files automatically upgrade to v2 on first load, with alt=0 (ground level default).
Why Altitude Matters:
/get-desired-launch-positionsPurpose: Calculate GPS coordinates for each drone’s intended launch position.
Method: GET
Response Structure:
{
"success": true,
"origin": {
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"source": "drone_telemetry"
},
"positions": [
{
"hw_id": "1",
"pos_id": "1",
"config_north": 10.5,
"config_east": 5.2,
"desired_lat": 37.774995,
"desired_lon": -122.419334,
"desired_alt": 45.5
}
],
"formation_stats": {
"total_drones": 10,
"extent_north_south": 25.3,
"extent_east_west": 18.7,
"max_distance_from_origin": 31.2,
"formation_diameter": 62.4
},
"heading": 0
}
Coordinate Conversion:
Uses pymap3d.ned2geodetic() for coordinate transformations:
import pymap3d as pm
launch_lat, launch_lon, launch_alt = pm.ned2geodetic(
config_north, # meters north of origin
config_east, # meters east of origin
0, # altitude offset (0 for ground level)
origin_lat, # formation origin latitude
origin_lon, # formation origin longitude
origin_alt # formation origin altitude MSL
)
Why pymap3d?
/get-position-deviationsPurpose: Professional position monitoring with GPS quality and status.
Method: GET
Response Structure:
{
"success": true,
"origin": {
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5
},
"deviations": {
"1": {
"hw_id": "1",
"expected": {
"lat": 37.774995,
"lon": -122.419334,
"alt": 45.5,
"north": 10.5,
"east": 5.2
},
"current": {
"lat": 37.774993,
"lon": -122.419332,
"alt": 46.2,
"north": 10.3,
"east": 5.1,
"gps_quality": "excellent",
"satellites": 18,
"hdop": 0.7
},
"deviation": {
"north": -0.2,
"east": -0.1,
"horizontal": 0.22,
"vertical": 0.7,
"total_3d": 0.73
},
"status": "ok",
"message": "Position within acceptable tolerance"
}
},
"summary": {
"total_drones": 10,
"online": 8,
"status_counts": {
"ok": 6,
"warning": 2,
"error": 0,
"no_telemetry": 2
},
"best_deviation": 0.15,
"worst_deviation": 2.34,
"average_deviation": 0.87
},
"timestamp": "2025-11-03T10:30:45.123456"
}
GPS Quality Classification:
def classify_gps_quality(satellites, hdop):
if satellites >= 10 and hdop <= 1.0:
return "excellent"
elif satellites >= 8 and hdop <= 2.0:
return "good"
elif satellites >= 6 and hdop <= 5.0:
return "fair"
elif satellites >= 4:
return "poor"
else:
return "no_fix"
Status Classification:
def classify_status(horizontal_deviation):
if horizontal_deviation < 2.0:
return "ok" # Green
elif horizontal_deviation < 5.0:
return "warning" # Orange
else:
return "error" # Red
Deviation Calculations:
# Horizontal deviation (2D)
horizontal_deviation = sqrt(north_dev² + east_dev²)
# Vertical deviation (altitude)
vertical_deviation = abs(expected_alt - current_alt)
# Total 3D deviation
total_3d_deviation = sqrt(north_dev² + east_dev² + vertical_dev²)
/set-origin and /get-originBoth now support altitude field with backwards compatibility.
Set Origin:
POST /set-origin
{
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5, # Optional, defaults to 0
"alt_source": "manual" # "manual", "drone_telemetry", or "gps_lock"
}
Get Origin:
GET /get-origin
Response:
{
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"alt_source": "drone_telemetry",
"timestamp": "2025-11-03T10:30:45.123456",
"version": 2
}
New Features:
Manual Mode UI:
<label style=>
Altitude MSL (optional, meters):
<input
type="number"
step="0.1"
value={altitude}
onChange={(e) => setAltitude(e.target.value)}
placeholder="Ground level (default: 0m)"
/>
</label>
Drone Mode Auto-Capture:
useEffect(() => {
if (mode === 'drone' && selectedDrone) {
const telemetry = telemetryData[selectedDrone.hw_id];
if (telemetry?.altitude !== undefined) {
setAltitude(telemetry.altitude.toString());
setAltitudeSource('drone_telemetry');
}
}
}, [mode, selectedDrone, telemetryData]);
Purpose: Tabbed interface for Launch Plot and Position Monitoring.
Features:
Tab Structure:
<PositionTabs>
<Tab name="launch">
📍 Launch Plot
<InitialLaunchPlot />
</Tab>
<Tab name="deviation">
📊 Position Monitoring [🟡2] [🔴0]
<DeviationView />
</Tab>
</PositionTabs>
Badge Logic:
const warnings = Object.values(deviationData.deviations || {})
.filter(d => d.status === 'warning').length;
const errors = Object.values(deviationData.deviations || {})
.filter(d => d.status === 'error').length;
Purpose: Real-time position monitoring with expected vs actual visualization.
Key Features:
Auto-Refresh Mechanism:
useEffect(() => {
if (!autoRefresh || !onRefresh) return;
const interval = setInterval(() => {
onRefresh();
setLastUpdate(new Date());
}, 5000); // Refresh every 5 seconds
return () => clearInterval(interval);
}, [autoRefresh, onRefresh]);
Three-Layer Plotly Visualization:
marker: {
size: 18,
color: '#3498db',
symbol: 'circle'
}
marker: {
size: 24,
color: statusColors[status], // green/orange/red/gray
symbol: 'circle-open',
line: { width: 4 }
}
mode: 'lines',
line: {
color: 'rgba(231, 76, 60, 0.5)',
width: 2
}
Rich Hover Tooltips:
hovertemplate:
'Drone: %{customdata.hw_id}<br>' +
'Expected: (%{customdata.exp_n:.2f}m N, %{customdata.exp_e:.2f}m E)<br>' +
'Current: (%{customdata.cur_n:.2f}m N, %{customdata.cur_e:.2f}m E)<br>' +
'Deviation: %{customdata.deviation:.2f}m<br>' +
'GPS: %{customdata.satellites} sats, HDOP %{customdata.hdop:.1f}<br>' +
'Quality: %{customdata.gps_quality}<br>' +
'<extra></extra>'
Summary Statistics Header:
<div className="deviation-summary">
<StatCard type="success" value={online} label="Online" />
<StatCard type="success" value={okCount} label="OK" />
<StatCard type="warning" value={warningCount} label="Warnings" />
<StatCard type="error" value={errorCount} label="Errors" />
<StatCard value={avgDeviation} label="Avg Dev (m)" />
<StatCard value={worstDeviation} label="Worst (m)" />
</div>
Integration:
import PositionTabs from '../components/PositionTabs';
// Manual refresh handler
const handleManualRefresh = () => {
if (!originAvailable) {
toast.warning('Origin must be set before fetching position deviations.');
return;
}
const backendURL = getBackendURL(process.env.REACT_APP_FLASK_PORT || '5000');
axios.get(`${backendURL}/get-position-deviations`)
.then((response) => setDeviationData(response.data))
.catch((error) => {
console.error('Error fetching position deviations:', error);
toast.error('Failed to refresh position data.');
});
};
// Replace InitialLaunchPlot with PositionTabs
<PositionTabs
drones={configData}
deviationData={deviationData}
origin={origin}
forwardHeading={forwardHeading}
onDroneClick={setEditingDroneId}
onRefresh={handleManualRefresh}
/>
Design Token System:
DesignTokens.css patternsKey Styling Features:
PositionTabs.css:
DeviationView.css:
Color Coding:
.marker.current.ok { border-color: #27ae60; } /* Green */
.marker.current.warning { border-color: #f39c12; } /* Orange */
.marker.current.error { border-color: #e74c3c; } /* Red */
Commit 1: 17a0d07c - feat: Fix origin system coordinate bugs and add altitude support
Commit 2: 3b29fa9d - feat: Add professional position monitoring UI with tabbed interface
The drone_show.py script orchestrates autonomous drone shows with these key phases:
1. Initialization → 2. Pre-flight → 3. Arm/Offboard → 4. Initial Climb → 5. Trajectory → 6. Landing
Function: read_config()
def read_config(filename: str) -> Drone:
"""
Read the drone configuration from a CSV file.
This CSV is assumed to store real NED coordinates directly:
- initial_x => North
- initial_y => East
"""
# ...
initial_x = float(row["x"]) # North
initial_y = float(row["y"]) # East
drone = Drone(
hw_id, pos_id,
initial_x, initial_y, # ← Launch offsets
ip, mavlink_port, debug_port, gcs_ip
)
What It Does:
x (North) and y (East) as launch position offsetsCurrent Assumption: Operator places drone exactly at (initial_x, initial_y).
Function: read_trajectory_file()
Two Modes Controlled by auto_launch_position Parameter:
Mode A: Auto Launch Position = True
if auto_launch_position:
# Extract initial positions from first waypoint
init_n, init_e, init_d = extract_initial_positions(rows[0])
# Adjust ALL waypoints so first waypoint becomes (0,0,0)
waypoints = adjust_waypoints(waypoints, init_n, init_e, init_d)
Mode B: Auto Launch Position = False (DEFAULT)
else:
# Use config.csv initial_x, initial_y
# Shift trajectory so it starts from config position
waypoints = adjust_waypoints(waypoints, initial_x, initial_y, 0.0)
Critical Insight: Both modes zero out offsets by subtracting initial positions. This means trajectory execution always starts from (0,0,0) in NED frame.
Function: compute_position_drift()
async def compute_position_drift():
"""
Compute initial position drift using LOCAL_POSITION_NED from the drone's API.
The NED origin is automatically set when the drone arms (matches GPS_GLOBAL_ORIGIN).
Returns:
PositionNedYaw: Drift in NED coordinates or None if unavailable
"""
response = requests.get(
f"http://localhost:{Params.drones_flask_port}/get-local-position-ned",
timeout=2
)
if response.status_code == 200:
ned_data = response.json()
drift = PositionNedYaw(
north_m=ned_data['x'], # How far north from PX4's origin
east_m=ned_data['y'], # How far east from PX4's origin
down_m=ned_data['z'], # How far down from PX4's origin
yaw_deg=0.0
)
return drift
What This Captures:
ENABLE_INITIAL_POSITION_CORRECTION = True, this drift is added to all waypointsCurrent Behavior (if enabled):
if Params.ENABLE_INITIAL_POSITION_CORRECTION and initial_position_drift:
px = raw_px + initial_position_drift.north_m # Shift trajectory
py = raw_py + initial_position_drift.east_m
pz = raw_pz + initial_position_drift.down_m
Problem: This correction is based on PX4’s auto-set origin, not the formation origin we want.
Function: pre_flight_checks()
async def pre_flight_checks(drone: System):
"""
Perform pre-flight checks to ensure the drone is ready for flight, including:
- Checking the health of the global and home position via MAVSDK
- Fetching the GPS global origin using MAVSDK when health is valid
"""
# ...
# Get PX4's GPS origin
origin = await drone.telemetry.get_gps_global_origin()
gps_origin = {
'latitude': origin.latitude_deg,
'longitude': origin.longitude_deg,
'altitude': origin.altitude_m
}
# This is PX4's internal origin, NOT formation origin
return gps_origin
Critical Distinction:
PX4 GPS Origin (gps_origin returned here):
├─ Set by PX4 at first GPS lock or arming
├─ Used for LOCAL_POSITION_NED ↔ GPS conversions internally
└─ NOT under our control
Formation Origin (from origin.json):
├─ Set by operator via OriginModal
├─ Defines where (0,0,0) should be for the formation
└─ Used to calculate expected GPS positions
These are NOT the same and must not be confused!
In perform_trajectory() function:
# Calculate GPS coordinates from NED setpoint
lla_lat, lla_lon, lla_alt = pm.ned2geodetic(
px, py, pz, # NED position from CSV
launch_lat, launch_lon, launch_alt # Drone's launch GPS position
)
if Params.USE_GLOBAL_SETPOINTS:
# Send GLOBAL setpoint (lat, lon, alt, yaw)
gp = PositionGlobalYaw(
lla_lat, lla_lon, lla_alt,
raw_yaw,
PositionGlobalYaw.AltitudeType.AMSL
)
await drone.offboard.set_position_global(gp)
else:
# Send LOCAL NED setpoint
ln = PositionNedYaw(px, py, pz, raw_yaw)
await drone.offboard.set_position_ned(ln)
What This Does:
USE_GLOBAL_SETPOINTS = False: Sends local NED positions (default)USE_GLOBAL_SETPOINTS = True: Converts to GPS and sends global positionsLaunch Position Capture (Lines 1432-1447):
# Capture launch position from telemetry
async for pos in drone.telemetry.position():
launch_lat = pos.latitude_deg
launch_lon = pos.longitude_deg
launch_alt = pos.absolute_altitude_m
break
Current Behavior:
launch_lat/lon/alt = where the drone currently is when script startsFrom src/params.py (referenced but not shown):
# Positioning modes
AUTO_LAUNCH_POSITION = False # Use config.csv positions vs first waypoint
ENABLE_INITIAL_POSITION_CORRECTION = True # Apply PX4 origin drift
# Offboard mode
USE_GLOBAL_SETPOINTS = False # Local NED vs Global GPS setpoints
# Global position requirements
REQUIRE_GLOBAL_POSITION = True # Require GPS lock for pre-flight
Current Flow (Simplified):
1. Operator places drone (assume it's at correct position)
2. Script reads config.csv: initial_x=10.5, initial_y=5.2
3. Script captures launch_lat/lon/alt = wherever drone is now
4. Trajectory CSV is loaded and adjusted to start from (0,0,0)
5. During execution:
- If USE_GLOBAL_SETPOINTS=False: Sends NED positions relative to launch point
- If USE_GLOBAL_SETPOINTS=True: Converts NED to GPS using launch_lat/lon/alt
6. Result: Show executes from wherever drones actually are
If Drones Are Misplaced:
New Mode: Global Correction (Origin-Based Execution)
1. Operator places drones (doesn't need to be perfect)
2. Script reads config.csv: initial_x=10.5, initial_y=5.2
3. Script reads formation origin: origin_lat, origin_lon, origin_alt
4. Calculate expected GPS position for this drone:
expected_lat, expected_lon, expected_alt = ned2geodetic(
initial_x, initial_y, 0,
origin_lat, origin_lon, origin_alt
)
5. Capture actual launch position: launch_lat, launch_lon, launch_alt
6. Calculate position error:
error_north, error_east, error_down = geodetic2ned(
launch_lat, launch_lon, launch_alt,
expected_lat, expected_lon, expected_alt
)
7. During initial climb: Move from current position
8. After initial climb: Correct to expected position
9. Execute trajectory from corrected position
Benefits:
Integrate the origin system into drone_show.py to enable origin-based execution mode where drones automatically correct to their intended launch positions using global GPS coordinates.
USE_ORIGIN_CORRECTION
USE_ORIGIN_CORRECTION=FalseProblem: Multiple coordinate systems with similar names.
Solutions:
formation_origin, px4_origin, launch_positionProblem: When to apply position corrections?
Constraints:
Proposed Approach:
1. Arm at current position (wherever drone is)
2. Initial climb: Vertical only, maintain horizontal position
3. At climb completion: Smooth transition to expected position
4. Execute trajectory from corrected position
Problem: GPS is not perfectly accurate.
Considerations:
Mitigations:
Problem: Large corrections could be dangerous.
Requirements:
Implementation:
MAX_CORRECTION_DISTANCE = 10.0 # meters
correction_distance = sqrt(error_north² + error_east²)
if correction_distance > MAX_CORRECTION_DISTANCE:
logger.error(f"Correction distance {correction_distance:.2f}m exceeds "
f"maximum {MAX_CORRECTION_DISTANCE}m. Aborting.")
raise ValueError("Position correction too large - likely config error")
Problem: Can’t break existing shows.
Requirements:
Current Workflow:
1. Deploy drones on ground in formation
2. Operator visually places each drone at correct position
3. Power on drones
4. Launch show
New Workflow (Optional):
1. Set formation origin (once, from GCS)
2. Deploy drones roughly in formation (doesn't need to be perfect)
3. Power on drones
4. System auto-calculates corrections
5. Monitor position deviations in UI
6. Launch show (drones auto-correct during initial climb)
Problem: Drones in field may not have reliable internet.
Solutions:
origin.json fileProblem: Different shows, different origins.
Solutions:
Critical Rule: The execution loop in perform_trajectory() is battle-tested. Don’t change core flight logic unless absolutely necessary.
Allowed Changes:
Forbidden Changes:
Current Redundant Concepts:
- PX4 origin vs formation origin (both called "origin")
- launch_position vs initial_position vs home_position
- auto_launch_position vs ENABLE_INITIAL_POSITION_CORRECTION
- USE_GLOBAL_SETPOINTS vs USE_ORIGIN_CORRECTION (new)
Goal: Clear, distinct terms for each concept.
Every function dealing with coordinates must document:
Template:
def calculate_expected_position(config_north: float, config_east: float,
formation_origin: dict) -> dict:
"""
Calculate expected GPS position from formation-relative coordinates.
Coordinate System: Uses formation origin as (0,0,0) reference.
Args:
config_north (float): North offset in meters from formation origin
config_east (float): East offset in meters from formation origin
formation_origin (dict): Formation origin with 'lat', 'lon', 'alt' keys
in WGS84 coordinates (degrees, meters MSL)
Returns:
dict: Expected GPS position with keys:
- 'lat': Latitude in degrees (WGS84)
- 'lon': Longitude in degrees (WGS84)
- 'alt': Altitude in meters MSL
Example:
config_north = 10.5 # 10.5m north of origin
config_east = 5.2 # 5.2m east of origin
origin = {'lat': 37.7749, 'lon': -122.4194, 'alt': 45.5}
expected = calculate_expected_position(config_north, config_east, origin)
# Returns: {'lat': 37.774995, 'lon': -122.419334, 'alt': 45.5}
"""
┌─────────────────────────────────────────────────────────────────┐
│ OPERATOR (GCS) │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ Dashboard UI (React) │ │
│ │ ├─ OriginModal: Set formation origin (lat/lon/alt) │ │
│ │ ├─ LaunchPlot: Visualize formation layout │ │
│ │ └─ DeviationView: Monitor real-time positions │ │
│ └────────────────────────────────────────────────────────────┘ │
│ ↓ HTTP API │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ GCS Server (Flask Python) │ │
│ │ ├─ /set-origin: Save formation origin │ │
│ │ ├─ /get-origin: Retrieve formation origin │ │
│ │ ├─ /get-desired-launch-positions: Calculate GPS coords │ │
│ │ └─ /get-position-deviations: Monitor deviations │ │
│ │ │ │
│ │ Data Store: origin.json (formation origin) │ │
│ │ config.csv (drone offsets) │ │
│ └────────────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
↓ WiFi/Network
┌─────────────────────────────────────────────────────────────────┐
│ DRONE (Raspberry Pi) │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ drone_show.py (Python) [PHASE 2] │ │
│ │ ├─ Read origin.json (formation origin) [NEW] │ │
│ │ ├─ Read config.csv (this drone's offsets) │ │
│ │ ├─ Calculate expected GPS position [NEW] │ │
│ │ ├─ Capture actual GPS position │ │
│ │ ├─ Calculate correction vector [NEW] │ │
│ │ └─ Execute trajectory with corrections [NEW] │ │
│ └────────────────────────────────────────────────────────────┘ │
│ ↓ MAVLink │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ MAVSDK Server (gRPC ↔ MAVLink bridge) │ │
│ │ ├─ Telemetry: GPS position, altitude, heading │ │
│ │ ├─ Offboard: Send position/velocity setpoints │ │
│ │ └─ Action: Arm, disarm, land │ │
│ └────────────────────────────────────────────────────────────┘ │
│ ↓ MAVLink │
│ ┌────────────────────────────────────────────────────────────┐ │
│ │ PX4 Autopilot (Flight Controller) │ │
│ │ ├─ GPS: Provides position, altitude, quality │ │
│ │ ├─ EKF: Fuses GPS + IMU + baro for state estimate │ │
│ │ ├─ Position Controller: Tracks setpoints │ │
│ │ └─ Sets GPS origin automatically at arming │ │
│ └────────────────────────────────────────────────────────────┘ │
└─────────────────────────────────────────────────────────────────┘
┌──────────────────────────────────────────────────────────────────┐
│ STEP 1: Initialization (Before Flight) │
└──────────────────────────────────────────────────────────────────┘
Operator sets formation origin via OriginModal:
origin = {lat: 37.7749, lon: -122.4194, alt: 45.5}
Saved to: gcs-server/origin.json
↓
┌──────────────────────────────────────────────────────────────────┐
│ STEP 2: Drone Startup (drone_show.py) │
└──────────────────────────────────────────────────────────────────┘
Read config.csv for this drone (HW_ID=1):
config_north = 10.5m (x column)
config_east = 5.2m (y column)
Read origin.json:
origin_lat = 37.7749
origin_lon = -122.4194
origin_alt = 45.5
Calculate expected GPS position:
expected_lat, expected_lon, expected_alt = ned2geodetic(
10.5, 5.2, 0,
37.7749, -122.4194, 45.5
)
→ expected_lat = 37.774995
→ expected_lon = -122.419334
→ expected_alt = 45.5
↓
┌──────────────────────────────────────────────────────────────────┐
│ STEP 3: Pre-Flight (before arming) │
└──────────────────────────────────────────────────────────────────┘
Wait for GPS lock (pre_flight_checks)
Capture actual launch position:
launch_lat = 37.774990 (drone is 5m south of expected)
launch_lon = -122.419330
launch_alt = 46.2
Calculate position error:
error_north, error_east, error_down = geodetic2ned(
37.774990, -122.419330, 46.2,
37.774995, -122.419334, 45.5
)
→ error_north = -5.0m (5m south of expected)
→ error_east = -0.4m (0.4m west of expected)
→ error_down = 0.7m (0.7m above expected)
Calculate correction distance:
correction_distance = sqrt(5.0² + 0.4²) = 5.02m
Check safety threshold:
if correction_distance > MAX_CORRECTION_DISTANCE:
ABORT (config error or wrong origin)
else:
LOG: "Position correction required: 5.02m"
↓
┌──────────────────────────────────────────────────────────────────┐
│ STEP 4: Arming and Initial Climb │
└──────────────────────────────────────────────────────────────────┘
Arm drone at current position
PX4 sets its GPS origin = launch_lat/lon/alt
Initial climb (vertical only):
- Maintain horizontal position (current north/east)
- Climb to INITIAL_CLIMB_ALTITUDE_THRESHOLD (e.g., 5m)
- Use LOCAL_NED mode for climb
↓
┌──────────────────────────────────────────────────────────────────┐
│ STEP 5: Position Correction (after initial climb) │
└──────────────────────────────────────────────────────────────────┘
Calculate correction waypoint:
If USE_ORIGIN_CORRECTION=True:
target_lat = expected_lat
target_lon = expected_lon
target_alt = expected_alt
Smooth transition to corrected position:
- Use GLOBAL setpoint mode for correction
- Move horizontally at safe speed (e.g., 2 m/s)
- Maintain altitude during transition
Wait until position reached (tolerance: 1m)
↓
┌──────────────────────────────────────────────────────────────────┐
│ STEP 6: Trajectory Execution (from corrected position) │
└──────────────────────────────────────────────────────────────────┘
Now at expected position, execute trajectory:
- All CSV waypoints are relative to (0,0,0)
- (0,0,0) now corresponds to expected GPS position
- Formation executes correctly
If USE_GLOBAL_SETPOINTS=True:
Convert each NED waypoint to GPS:
waypoint_lat, waypoint_lon, waypoint_alt = ned2geodetic(
csv_north, csv_east, csv_down,
expected_lat, expected_lon, expected_alt ← Use expected, not launch
)
Execute show...
mavsdk_drone_show/
├── drone_show.py # Main execution script [PHASE 2 CHANGES]
├── config.csv # Drone configuration (x=North, y=East)
│
├── gcs-server/ # Ground Control Server
│ ├── app.py # Flask server
│ ├── routes.py # API endpoints [PHASE 1 COMPLETE]
│ ├── origin.py # Origin management [PHASE 1 COMPLETE]
│ └── origin.json # Formation origin storage
│
├── app/dashboard/drone-dashboard/ # React UI
│ ├── src/
│ │ ├── components/
│ │ │ ├── OriginModal.js # Set origin [PHASE 1 COMPLETE]
│ │ │ ├── PositionTabs.js # Tab interface [PHASE 1 COMPLETE]
│ │ │ ├── DeviationView.js # Position monitoring [PHASE 1 COMPLETE]
│ │ │ └── InitialLaunchPlot.js # Formation plot
│ │ ├── pages/
│ │ │ └── MissionConfig.js # Main config page [PHASE 1 COMPLETE]
│ │ └── styles/
│ │ ├── PositionTabs.css # [PHASE 1 COMPLETE]
│ │ └── DeviationView.css # [PHASE 1 COMPLETE]
│
├── src/
│ ├── params.py # Configuration parameters [PHASE 2 CHANGES]
│ ├── led_controller.py
│ └── ...
│
├── shapes/ # Trajectory CSV files
│ └── swarm/processed/ # Per-drone trajectories
│ └── Drone 1.csv # px=North, py=East, pz=Down (NED)
│
└── docs/
└── ORIGIN_SYSTEM_IMPLEMENTATION_GUIDE.md # This document
Set the formation origin coordinates.
Request Body:
{
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"alt_source": "drone_telemetry"
}
Response:
{
"success": true,
"message": "Origin saved successfully"
}
Retrieve the current formation origin.
Response:
{
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"alt_source": "drone_telemetry",
"timestamp": "2025-11-03T10:30:45.123456",
"version": 2
}
Calculate GPS coordinates for all drone launch positions.
Query Parameters:
heading (optional): Formation rotation in degrees (0-359)Response:
{
"success": true,
"origin": {
"lat": 37.7749,
"lon": -122.4194,
"alt": 45.5,
"source": "drone_telemetry"
},
"positions": [
{
"hw_id": "1",
"pos_id": "1",
"config_north": 10.5,
"config_east": 5.2,
"desired_lat": 37.774995,
"desired_lon": -122.419334,
"desired_alt": 45.5
}
],
"formation_stats": {
"total_drones": 10,
"extent_north_south": 25.3,
"extent_east_west": 18.7,
"max_distance_from_origin": 31.2,
"formation_diameter": 62.4
},
"heading": 0
}
Monitor real-time position deviations.
Response: See Phase 1 implementation section for full structure.
Setup:
# Start SITL simulation for 10 drones
./multiple_sitl/multiple_sitl.sh 10
# Start GCS server
cd gcs-server && python app.py
# Start dashboard
cd app/dashboard/drone-dashboard && npm start
Test Cases:
TC-1: Origin Not Set (Fallback Mode)
- Condition: origin.json does not exist
- Expected: Drone executes using current behavior
- Verify: Show runs without errors
TC-2: Origin Set, Correction Disabled
- Condition: origin.json exists, USE_ORIGIN_CORRECTION=False
- Expected: Drone ignores origin, uses current behavior
- Verify: Show runs as before
TC-3: Origin Set, Perfect Placement
- Condition: origin.json exists, USE_ORIGIN_CORRECTION=True
- Drones placed exactly at expected positions
- Expected: Correction distance ~0m, no correction applied
- Verify: Show executes normally
TC-4: Origin Set, Small Placement Error
- Condition: origin.json exists, USE_ORIGIN_CORRECTION=True
- Simulate 2m east offset for drone 1
- Expected:
- Log shows "Position correction required: 2.0m"
- Drone corrects after initial climb
- Formation executes correctly
- Verify: Final positions match expected within 1m
TC-5: Origin Set, Large Placement Error
- Condition: origin.json exists, USE_ORIGIN_CORRECTION=True
- Simulate 15m offset (exceeds MAX_CORRECTION_DISTANCE)
- Expected: Script aborts with error message
- Verify: Drone does not take off
TC-6: GPS Quality Check
- Condition: Simulate poor GPS (4 satellites, HDOP=6.0)
- Expected: Script warns and falls back to local NED mode
- Verify: Correction not applied due to poor GPS quality
Pre-Flight Checklist:
During Flight Monitoring:
Post-Flight Validation:
EC-1: One Drone GPS Lost
- Scenario: Drone 3 loses GPS mid-flight
- Expected:
- Drone 3 status → "error"
- Other drones continue normally
- Drone 3 enters failsafe (hold position or RTL)
EC-2: Origin Updated Mid-Mission
- Scenario: Operator updates origin while show running
- Expected:
- Running drones ignore update
- New origin applies to next show only
EC-3: Network Loss
- Scenario: WiFi connection lost during flight
- Expected:
- Drones continue executing trajectory
- UI shows last known positions
- Telemetry resumes when connection restored
EC-4: Config.csv Mismatch
- Scenario: Drone has different HW_ID than expected
- Expected:
- Pre-flight check fails
- Error logged clearly
- Drone does not arm
P-1: Coordinate Conversion Performance
import time
import pymap3d as pm
# Benchmark ned2geodetic calls
start = time.time()
for i in range(10000):
lat, lon, alt = pm.ned2geodetic(
10.5, 5.2, 0,
37.7749, -122.4194, 45.5
)
end = time.time()
print(f"10000 conversions: {(end-start)*1000:.2f}ms")
# Expected: < 100ms total (< 0.01ms per conversion)
P-2: API Response Time
# Measure /get-position-deviations latency
time curl http://localhost:5000/get-position-deviations
# Expected: < 500ms for 10 drones
P-3: UI Refresh Rate
- DeviationView auto-refresh: 5 seconds
- Expected: No UI lag or freezing
- CPU usage: < 10% during refresh
| Term | Definition | Example |
|---|---|---|
| NED | North-East-Down coordinate system. Right-handed. X=North (forward), Y=East (right), Z=Down. | (10.5, 5.2, -3.0) = 10.5m north, 5.2m east, 3m up |
| LLA | Latitude-Longitude-Altitude (GPS coordinates). WGS84 geodetic. | (37.7749°, -122.4194°, 45.5m MSL) |
| AMSL | Above Mean Sea Level. Altitude reference. | 45.5m AMSL = 45.5 meters above sea level |
| WGS84 | World Geodetic System 1984. Global coordinate reference system. | Standard GPS coordinate system |
| Term | Definition | Notes |
|---|---|---|
| Formation Origin | GPS coordinates defining (0,0,0) for the drone formation. Set by operator. | Our new system from Phase 1 |
| PX4 GPS Origin | GPS coordinates where PX4 thinks (0,0,0) is. Auto-set at arming/first GPS lock. | NOT the same as formation origin! |
| Launch Position | Actual GPS coordinates where a drone is physically located at startup. | Captured from telemetry |
| Expected Position | GPS coordinates where a drone should be based on config.csv offsets. | Calculated from formation origin + offsets |
| Parameter | Type | Default | Description |
|---|---|---|---|
USE_ORIGIN_CORRECTION |
bool | False | [NEW] Enable origin-based position correction |
AUTO_LAUNCH_POSITION |
bool | False | Extract launch position from first trajectory waypoint |
USE_GLOBAL_SETPOINTS |
bool | False | Send global GPS setpoints vs local NED setpoints |
ENABLE_INITIAL_POSITION_CORRECTION |
bool | True | Apply PX4 origin drift correction |
REQUIRE_GLOBAL_POSITION |
bool | True | Require GPS lock for pre-flight checks |
| Term | Definition | Formula |
|---|---|---|
| Horizontal Deviation | 2D distance between expected and actual position | sqrt(north_error² + east_error²) |
| Vertical Deviation | Altitude difference | abs(expected_alt - actual_alt) |
| 3D Deviation | Total 3D distance | sqrt(north_error² + east_error² + down_error²) |
| Position Error | NED vector from expected to actual position | (error_north, error_east, error_down) |
| Term | Definition | Good Value |
|---|---|---|
| HDOP | Horizontal Dilution of Precision. Lower is better. | < 2.0 |
| Satellite Count | Number of GPS satellites tracked. | ≥ 8 |
| GPS Quality | Classification: excellent/good/fair/poor/no_fix | excellent |
| GPS Lock | GPS has valid 3D position fix | Required for flight |
| Status | Color | Horizontal Deviation | Action |
|---|---|---|---|
| ok | Green | < 2.0m | Normal operation |
| warning | Orange | 2.0m - 5.0m | Monitor closely |
| error | Red | > 5.0m | Investigate/abort |
| no_telemetry | Gray | N/A | No data received |
Before writing any code, the next AI should:
Decision 1: When to Apply Corrections?
Decision 2: How to Handle Correction Failures?
Decision 3: Parameter Consolidation
Phase 2.1: Foundation
USE_ORIGIN_CORRECTION parameter to params.pyload_formation_origin() functioncalculate_expected_position() functioncalculate_position_error() functionPhase 2.2: Position Correction Logic
arming_and_starting_offboard_mode() to calculate correctionsperform_trajectory()Phase 2.3: Global Setpoint Integration
USE_GLOBAL_SETPOINTS works with origin correctionPhase 2.4: Safety and Fallbacks
Phase 2.5: Testing and Validation
⚠️ DO NOT:
✅ DO:
Before starting implementation:
Phase 2 will be considered successful when:
def load_formation_origin() -> dict:
"""
Load formation origin from origin.json file.
Returns:
dict: Formation origin with keys 'lat', 'lon', 'alt', or None if not available
Example:
origin = load_formation_origin()
if origin:
print(f"Origin: {origin['lat']}, {origin['lon']}, {origin['alt']}m")
else:
print("No origin set - using fallback mode")
"""
logger = logging.getLogger(__name__)
origin_file = os.path.join('gcs-server', 'origin.json')
try:
if not os.path.exists(origin_file):
logger.warning(f"Origin file not found: {origin_file}")
return None
with open(origin_file, 'r') as f:
data = json.load(f)
# Validate required fields
if 'lat' not in data or 'lon' not in data:
logger.error("Origin file missing required fields (lat, lon)")
return None
origin = {
'lat': float(data['lat']),
'lon': float(data['lon']),
'alt': float(data.get('alt', 0.0)), # Default to 0 if not present
'source': data.get('alt_source', 'unknown'),
'version': data.get('version', 1)
}
logger.info(f"Formation origin loaded: lat={origin['lat']:.6f}, "
f"lon={origin['lon']:.6f}, alt={origin['alt']:.2f}m "
f"(source: {origin['source']})")
return origin
except Exception as e:
logger.exception(f"Error loading formation origin: {e}")
return None
def calculate_expected_position(config_north: float, config_east: float,
formation_origin: dict) -> dict:
"""
Calculate expected GPS position from formation-relative coordinates.
Coordinate System: Uses formation origin as (0,0,0) reference.
Args:
config_north (float): North offset in meters from formation origin
config_east (float): East offset in meters from formation origin
formation_origin (dict): Formation origin with 'lat', 'lon', 'alt' keys
Returns:
dict: Expected GPS position with keys 'lat', 'lon', 'alt'
Example:
config_north = 10.5 # From config.csv x column
config_east = 5.2 # From config.csv y column
origin = {'lat': 37.7749, 'lon': -122.4194, 'alt': 45.5}
expected = calculate_expected_position(config_north, config_east, origin)
# Returns: {'lat': 37.774995, 'lon': -122.419334, 'alt': 45.5}
"""
import pymap3d as pm
# Convert NED offset to GPS coordinates
expected_lat, expected_lon, expected_alt = pm.ned2geodetic(
config_north, # meters north of origin
config_east, # meters east of origin
0.0, # altitude offset (0 = same altitude as origin)
formation_origin['lat'], # origin latitude
formation_origin['lon'], # origin longitude
formation_origin['alt'] # origin altitude MSL
)
return {
'lat': expected_lat,
'lon': expected_lon,
'alt': expected_alt
}
def calculate_position_error(expected: dict, actual: dict) -> dict:
"""
Calculate position error in NED coordinates.
Args:
expected (dict): Expected GPS position {'lat', 'lon', 'alt'}
actual (dict): Actual GPS position {'lat', 'lon', 'alt'}
Returns:
dict: Position error with keys:
- 'north': Error in meters (positive = actual is north of expected)
- 'east': Error in meters (positive = actual is east of expected)
- 'down': Error in meters (positive = actual is below expected)
- 'horizontal': 2D distance in meters
- 'vertical': Altitude difference in meters
- 'total_3d': 3D distance in meters
Example:
expected = {'lat': 37.774995, 'lon': -122.419334, 'alt': 45.5}
actual = {'lat': 37.774990, 'lon': -122.419330, 'alt': 46.2}
error = calculate_position_error(expected, actual)
# Returns: {'north': -5.0, 'east': -0.4, 'down': 0.7,
# 'horizontal': 5.02, 'vertical': 0.7, 'total_3d': 5.07}
"""
import pymap3d as pm
import math
# Convert from expected to actual in NED coordinates
error_north, error_east, error_down = pm.geodetic2ned(
actual['lat'], actual['lon'], actual['alt'],
expected['lat'], expected['lon'], expected['alt']
)
# Calculate derived metrics
horizontal = math.sqrt(error_north**2 + error_east**2)
vertical = abs(error_down)
total_3d = math.sqrt(error_north**2 + error_east**2 + error_down**2)
return {
'north': error_north,
'east': error_east,
'down': error_down,
'horizontal': horizontal,
'vertical': vertical,
'total_3d': total_3d
}
async def validate_correction_safety(position_error: dict,
gps_quality: dict,
params) -> tuple:
"""
Validate that position correction is safe to apply.
Args:
position_error (dict): Position error from calculate_position_error()
gps_quality (dict): GPS quality metrics {'satellites', 'hdop'}
params: Parameters object with safety thresholds
Returns:
tuple: (is_safe: bool, message: str)
Safety Checks:
1. GPS quality sufficient (satellites >= 8, HDOP <= 2.0)
2. Correction distance within limits (< MAX_CORRECTION_DISTANCE)
3. Vertical correction reasonable (< MAX_VERTICAL_CORRECTION)
Example:
is_safe, msg = await validate_correction_safety(error, gps, Params)
if not is_safe:
logger.error(f"Correction unsafe: {msg}")
sys.exit(1)
"""
logger = logging.getLogger(__name__)
# Check 1: GPS Quality
min_satellites = params.MIN_GPS_SATELLITES # e.g., 8
max_hdop = params.MAX_GPS_HDOP # e.g., 2.0
satellites = gps_quality.get('satellites', 0)
hdop = gps_quality.get('hdop', 99.9)
if satellites < min_satellites:
return False, f"Insufficient GPS satellites: {satellites} < {min_satellites}"
if hdop > max_hdop:
return False, f"GPS HDOP too high: {hdop} > {max_hdop}"
# Check 2: Horizontal Correction Distance
max_horizontal = params.MAX_CORRECTION_DISTANCE # e.g., 10.0 meters
horizontal = position_error['horizontal']
if horizontal > max_horizontal:
return False, (f"Correction distance too large: {horizontal:.2f}m > "
f"{max_horizontal}m (likely config error)")
# Check 3: Vertical Correction
max_vertical = params.MAX_VERTICAL_CORRECTION # e.g., 5.0 meters
vertical = position_error['vertical']
if vertical > max_vertical:
return False, (f"Altitude correction too large: {vertical:.2f}m > "
f"{max_vertical}m (check origin altitude)")
# All checks passed
logger.info(f"Position correction validated: {horizontal:.2f}m horizontal, "
f"{vertical:.2f}m vertical, GPS quality OK "
f"({satellites} sats, HDOP {hdop:.1f})")
return True, "Correction within safe parameters"
For comprehensive information about the completed Phase 2 implementation and control modes:
This guide represents the complete knowledge transfer from Phase 1 (origin system implementation) to Phase 2 (drone_show.py integration). The next AI working on this project should:
The goal is a robust, safe, well-documented drone show execution system that works perfectly in both modes:
Good luck with Phase 2! 🚁
Document End
Last Updated: November 3, 2025 Next Review: Before Phase 2 Implementation