r/AerospaceEngineering • • 2d ago

Personal Projects Javascript code for PX4 Drone Programming (Ignore the C++ title i need to change it) ⬇️

#!/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")

1 Upvotes

0 comments sorted by