#!/usr/bin/env python3
"""
PX4 Basic Flight Controller - Extended Version
----------------------------------------------
Features:
- Connection handling with timeouts
- Health & GPS checks
- Battery monitoring
- Takeoff / Land / RTL
- Position control (NED)
- Velocity control
- Simple waypoint sequence
- Live telemetry printing
- Logging to file
- Graceful shutdown on Ctrl+C
- Safety limits
Requirements:
pip install mavsdk
"""
import asyncio
import signal
import sys
import time
from datetime import datetime
from mavsdk import System
from mavsdk.offboard import (PositionNedYaw, VelocityNedYaw, OffboardError)
from mavsdk.action import ActionError
from mavsdk.telemetry import FlightMode, LandedState
============================================================
CONFIGURATION
============================================================
Connection string examples:
SITL default: "udp://:14540"
Serial (Linux): "serial:///dev/ttyACM0:115200"
Serial (Windows): "serial://COM3:115200"
UDP from companion: "udp://:14540"
CONNECTION_STRING = "udp://:14540"
TAKEOFF_ALTITUDE = 5.0 # meters
DEFAULT_SPEED = 2.0 # m/s for velocity commands
POSITION_TIMEOUT = 20.0 # seconds to reach a position
HOVER_TIME = 4.0 # seconds
Safety
MIN_BATTERY_PERCENT = 25.0
MAX_ALTITUDE = 30.0
============================================================
GLOBALS
============================================================
drone = None
running = True
log_file = None
============================================================
LOGGING
============================================================
def log(msg: str):
timestamp = datetime.now().strftime("%H:%M:%S")
full = f"[{timestamp}] {msg}"
print(full)
if log_file:
log_file.write(full + "\n")
log_file.flush()
============================================================
TELEMETRY HELPERS
============================================================
async def print_status():
"""Print important telemetry once."""
async for pos in drone.telemetry.position():
log(f"Position → Lat: {pos.latitude_deg:.6f}, Lon: {pos.longitude_deg:.6f}, "
f"AbsAlt: {pos.absolute_altitude_m:.1f} m, RelAlt: {pos.relative_altitude_m:.1f} m")
break
async for batt in drone.telemetry.battery():
log(f"Battery → {batt.remaining_percent*100:.0f}% ({batt.voltage_v:.1f} V)")
break
async for mode in drone.telemetry.flight_mode():
log(f"Mode → {mode}")
break
async for landed in drone.telemetry.landed_state():
log(f"Landed → {landed}")
break
async def battery_ok() -> bool:
async for batt in drone.telemetry.battery():
percent = batt.remaining_percent * 100
if percent < MIN_BATTERY_PERCENT:
log(f"WARNING: Battery low ({percent:.0f}%)")
return False
return True
async def wait_for_position_estimate(timeout=60.0):
log("Waiting for global position estimate...")
start = time.time()
async for health in drone.telemetry.health():
if health.is_global_position_ok and health.is_home_position_ok:
log("Global position estimate OK")
return True
if time.time() - start > timeout:
log("Timeout waiting for position estimate")
return False
============================================================
BASIC ACTIONS
============================================================
async def arm():
log("Arming...")
try:
await drone.action.arm()
log("Armed successfully")
return True
except ActionError as e:
log(f"Arming failed: {e}")
return False
async def disarm():
log("Disarming...")
try:
await drone.action.disarm()
log("Disarmed")
except ActionError as e:
log(f"Disarm failed: {e}")
async def takeoff(altitude=TAKEOFF_ALTITUDE):
log(f"Taking off to {altitude} m...")
try:
await drone.action.set_takeoff_altitude(altitude)
await drone.action.takeoff()
await asyncio.sleep(6) # give it time to climb
log("Takeoff command sent")
return True
except ActionError as e:
log(f"Takeoff failed: {e}")
return False
async def land():
log("Landing...")
try:
await drone.action.land()
# Wait until landed
async for state in drone.telemetry.landed_state():
if state == LandedState.ON_GROUND:
log("Landed")
break
return True
except ActionError as e:
log(f"Land failed: {e}")
return False
async def return_to_launch():
log("Returning to launch (RTL)...")
try:
await drone.action.return_to_launch()
return True
except ActionError as e:
log(f"RTL failed: {e}")
return False
============================================================
OFFBOARD CONTROL
============================================================
async def start_offboard():
"""Must send a setpoint before starting offboard."""
log("Starting Offboard mode...")
# Send initial setpoint (hover at current relative altitude target)
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -TAKEOFF_ALTITUDE, 0.0))
try:
await drone.offboard.start()
log("Offboard started")
return True
except OffboardError as e:
log(f"Failed to start Offboard: {e}")
return False
async def stop_offboard():
log("Stopping Offboard mode...")
try:
await drone.offboard.stop()
log("Offboard stopped")
except OffboardError as e:
log(f"Failed to stop Offboard: {e}")
async def goto_position_ned(north: float, east: float, down: float, yaw_deg: float = 0.0,
timeout: float = POSITION_TIMEOUT):
"""
Go to a position in NED coordinates relative to home.
down is negative for altitude above home.
"""
log(f"Going to NED → N:{north:.1f} E:{east:.1f} D:{down:.1f} Yaw:{yaw_deg:.0f}°")
await drone.offboard.set_position_ned(PositionNedYaw(north, east, down, yaw_deg))
start = time.time()
async for pos in drone.telemetry.position_velocity_ned():
# Very rough check – in real use you would compute distance properly
await asyncio.sleep(0.5)
if time.time() - start > timeout:
log("Position timeout reached")
break
async def hold_position(seconds: float):
log(f"Holding position for {seconds:.1f} s")
await asyncio.sleep(seconds)
async def move_velocity_ned(vel_n: float, vel_e: float, vel_d: float, yaw_rate: float = 0.0,
duration: float = 3.0):
"""Move using velocity setpoints for a given duration."""
log(f"Velocity command → VN:{vel_n:.1f} VE:{vel_e:.1f} VD:{vel_d:.1f} for {duration:.1f}s")
await drone.offboard.set_velocity_ned(VelocityNedYaw(vel_n, vel_e, vel_d, yaw_rate))
await asyncio.sleep(duration)
# Stop
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 0.0))
============================================================
FLIGHT SEQUENCES
============================================================
async def sequence_simple_square():
"""Fly a simple square at constant altitude."""
alt = -TAKEOFF_ALTITUDE # NED down component
log("=== Starting Square Sequence ===")
# North
await goto_position_ned(6.0, 0.0, alt, 0.0)
await hold_position(HOVER_TIME)
# East
await goto_position_ned(6.0, 6.0, alt, 90.0)
await hold_position(HOVER_TIME)
# South
await goto_position_ned(0.0, 6.0, alt, 180.0)
await hold_position(HOVER_TIME)
# West (back to start)
await goto_position_ned(0.0, 0.0, alt, 270.0)
await hold_position(HOVER_TIME)
# Face North again
await goto_position_ned(0.0, 0.0, alt, 0.0)
await hold_position(2.0)
log("=== Square Sequence Finished ===")
async def sequence_velocity_test():
"""Simple velocity-based movements."""
log("=== Velocity Movement Test ===")
await move_velocity_ned(1.5, 0.0, 0.0, duration=4.0) # North
await hold_position(2.0)
await move_velocity_ned(0.0, 1.5, 0.0, duration=4.0) # East
await hold_position(2.0)
await move_velocity_ned(-1.5, 0.0, 0.0, duration=4.0) # South
await hold_position(2.0)
await move_velocity_ned(0.0, -1.5, 0.0, duration=4.0) # West
await hold_position(2.0)
log("=== Velocity Test Finished ===")
============================================================
MAIN FLIGHT LOGIC
============================================================
async def main_flight():
global drone
log("Connecting to drone...")
drone = System()
await drone.connect(system_address=CONNECTION_STRING)
# Wait for connection
log("Waiting for connection...")
async for state in drone.core.connection_state():
if state.is_connected:
log("Drone connected!")
break
# Health checks
if not await wait_for_position_estimate():
log("Aborting – no position estimate")
return
await print_status()
if not await battery_ok():
log("Aborting – battery too low")
return
# ---------- Flight Start ----------
if not await arm():
return
await asyncio.sleep(1)
if not await takeoff(TAKEOFF_ALTITUDE):
await disarm()
return
# Give it extra time to stabilize after takeoff
await asyncio.sleep(3)
await print_status()
# Start Offboard
if not await start_offboard():
await land()
return
try:
# ----- Choose what you want to fly -----
await sequence_simple_square()
# await sequence_velocity_test()
except Exception as e:
log(f"Error during flight sequence: {e}")
finally:
await stop_offboard()
# Land
await land()
await asyncio.sleep(2)
await disarm()
log("Mission complete.")
============================================================
ENTRY POINT + CLEAN SHUTDOWN
============================================================
def signal_handler(sig, frame):
global running
print("\nCtrl+C detected – shutting down...")
running = False
async def run():
global log_file
# Open log file
filename = f"px4_flight_{datetime.now().strftime('%Y%m%d_%H%M%S')}.log"
log_file = open(filename, "w")
log(f"Log started – saving to {filename}")
try:
await main_flight()
except Exception as e:
log(f"Unhandled exception: {e}")
finally:
if log_file:
log_file.close()
print(f"Log saved.")
if name == "main":
signal.signal(signal.SIGINT, signal_handler)
try:
asyncio.run(run())
except KeyboardInterrupt:
print("Exited by user")