uav_control.py~

from dronekit import connect, VehicleMode, LocationGlobalRelative
import time

def connect_drone(connection_string):
    """
    Connect to the drone using the provided connection string.
    """
    try:
        print(f"Connecting to drone on: {connection_string}")
        vehicle = connect(connection_string, wait_ready=True)
        return vehicle
    except Exception as e:
        print(f"Error connecting to drone: {e}")
        return None

def arm_and_takeoff(vehicle, target_altitude):
    """
    Arms the drone and takes off to the target altitude.
    """
    print("Basic pre-arm checks")
    while not vehicle.is_armable:
        print(" Waiting for vehicle to initialise...")
        time.sleep(1)

    print("Arming motors")
    vehicle.mode = VehicleMode("GUIDED")
    vehicle.armed = True

    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    print("Taking off!")
    vehicle.simple_takeoff(target_altitude)

    while True:
        print(f" Altitude: {vehicle.location.global_relative_frame.alt}")
        if vehicle.location.global_relative_frame.alt >= target_altitude * 0.95:
            print("Reached target altitude")
            break
        time.sleep(1)

def fly_to_point(vehicle, latitude, longitude, altitude):
    """
    Commands the drone to fly to a specified location.
    """
    point = LocationGlobalRelative(latitude, longitude, altitude)
    vehicle.simple_goto(point)

    # Ожидание прибытия (для простоты)
    time.sleep(30)

def land_drone(vehicle):
    """
    Commands the drone to land.
    """
    print("Landing")
    vehicle.mode = VehicleMode("LAND")

    while vehicle.armed:
        print(" Waiting for landing...")
        time.sleep(1)

    print("Landed")

def main():
    # Подключение к дрону (используем SITL для симуляции)
    connection_string = '127.0.0.1:14550'
    vehicle = connect_drone(connection_string)

    if vehicle is None:
        print("Failed to connect to the drone.")
        return

    try:
        arm_and_takeoff(vehicle, 10)

        # Пример координат (используйте реальные данные)
        target_latitude = vehicle.location.global_frame.lat + 0.0001
        target_longitude = vehicle.location.global_frame.lon + 0.0001

        fly_to_point(vehicle, target_latitude, target_longitude, 10)

        land_drone(vehicle)
    finally:
        vehicle.close()
        print("Connection closed.")

if __name__ == '__main__':
    main()