Waypoint navigáció python nyelven

A drónok automatikus, GPS-alapú navigációjához Python nyelven a legelterjedtebb és legbiztonságosabb megoldás a DroneKit-Python API használata, amely ArduPilot vagy PX4 alapú repülésvezérlőkkel kommunikál a MAVLink protokollon keresztül.Mivel Nyíregyháza és Debrecen távolsága légvonalban körülbelül 50 kilométer, Egy ilyen automatizált repülés végrehajtásához ipari, merevszárnyú vagy hibrid (VTOL) drónra, valamint a hatályos légügyi jogszabályok (eseti légtér, BVLOS engedélyek) szigorú betartására van szükség. Az alábbi Python programkód bemutatja, hogyan lehet automatizált küldetést (waypoint navigációt) programozni Nyíregyháza és Debrecen között valós kompatibilis drónon.

---------------

import time

from dronekit import connect, VehicleMode, LocationGlobalRelative, Command

from pymavlink import mavutil

# 1. Kapcsolódás a drónhoz (Szimulátor vagy soros/UDP port valós drón esetén)

# Szimulátorhoz: 'tcp:127.0.0.1:5760'

# Valós telemetriához pl.: '/dev/ttyUSB0' vagy 'udp:127.0.0.1:14550'

connection_string = 'tcp:127.0.0.1:5760'

print(f"Kapcsolódás a drónhoz: {connection_string}")

vehicle = connect(connection_string, wait_ready=True)

def arm_and_takeoff(target_altitude):

    """Felszállás a megadott magasságra GUIDED módban."""

    print("Rendszerellenőrzés...")

    while not vehicle.is_armable:

        print(" Várakozás a drón indíthatóságára (GPS fix, stb.)...")

        time.sleep(1)

    print("Mód átváltása: GUIDED")

    vehicle.mode = VehicleMode("GUIDED")

    vehicle.armed = True

    while not vehicle.armed:

        print(" Várakozás a motorok indítására...")

        time.sleep(1)

    print(f"Felszállás megkezdése... Célmagasság: {target_altitude} méter")

    vehicle.simple_takeoff(target_altitude)

    while True:

        print(f" Jelenlegi magasság: {vehicle.location.global_relative_frame.alt:.1f} m")

        if vehicle.location.global_relative_frame.alt >= target_altitude * 0.95:

            print("Célmagasság elérve.")

            break

        time.sleep(1)

def clear_mission():

    """Törli a korábbi repülési terveket."""

    cmds = vehicle.commands

    cmds.clear()

    cmds.upload()

def create_nyiregyhaza_debrecen_mission():

    """Létrehozza a Nyíregyháza -> Debrecen útvonalpontokat."""

    print("Útvonalterv elkészítése...")

    cmds = vehicle.commands

    

    # Koordináták definíciója (Nyíregyháza és Debrecen hozzávetőleges pontjai)

    # Magasság egységesen 120 méter a földfelszín felett (jogszabályi maximum nyílt kategóriában)

    altitude = 120.0 

    

    wp_nyiregyhaza = LocationGlobalRelative(47.9554, 21.7167, altitude)

    wp_debrecen = LocationGlobalRelative(47.5316, 21.6273, altitude)

    # Első parancs hozzáadása (dummy parancs a MAVLink protokoll miatt)

    cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 

                     mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 

                     wp_nyiregyhaza.lat, wp_nyiregyhaza.lon, altitude))

    # 1. Útvonalpont: Nyíregyháza indulási pont felett

    cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 

                     mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 

                     wp_nyiregyhaza.lat, wp_nyiregyhaza.lon, altitude))

    # 2. Útvonalpont: Debrecen érkezési pont

    cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 

                     mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, 

                     wp_debrecen.lat, wp_debrecen.lon, altitude))

    

    # 3. Leszállási parancs Debrecenben

    cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, 

                     mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0, 

                     wp_debrecen.lat, wp_debrecen.lon, 0))

    print("Útvonal feltöltése a repülésvezérlőre...")

    cmds.upload()

# --- FŐ PROGRAMFUTÁS ---

try:

    # 100 méteres biztonságos magasság kiválasztása

    flight_altitude = 100 

    

    # Korábbi küldetések törlése és az új útvonal feltöltése

    clear_mission()

    create_nyiregyhaza_debrecen_mission()

    

    # Felszállás végrehajtása

    arm_and_takeoff(flight_altitude)

    

    # Küldetés indítása AUTO módba váltással

    print("Mód átváltása: AUTO (Az automata repülés megkezdődik)")

    vehicle.mode = VehicleMode("AUTO")

    # Küldetés állapotának figyelése

    while vehicle.commands.next < len(vehicle.commands):

        current_wp = vehicle.commands.next

        print(f" Drón úton. Aktuális útvonalpont index: {current_wp}")

        time.sleep(5)

    print("A küldetés utolsó fázisa (leszállás) megkezdődött.")

except KeyboardInterrupt:

    print("\nRepülés megszakítva a felhasználó által! Váltás LOITER (lebegés) módba...")

    vehicle.mode = VehicleMode("LOITER")

except Exception as e:

    print(f"Hiba történt: {e}")

finally:

    # Kapcsolat lezárása

    print("Kapcsolat bontása a drónnal.")

    vehicle.close()

---------------