Description: This script automates a drone mission by performing the following steps: Connects to the drone using a specified connection string. Clears any existing missions to prevent conflicts. Adds a new mission with specified waypoints. Arms the drone and takes off to a target altitude. Monitors the mission progress through the waypoints. En…

drone_mission.py

import argparse
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative, Command
from pymavlink import mavutil


def connect_drone(connection_string):
    """
    Connects to the drone and waits until the connection is established.
    """
    print(f"Connecting to the drone on {connection_string}...")
    vehicle = connect(connection_string, wait_ready=True)
    print("Connected to the drone successfully!")
    return vehicle


def clear_mission(vehicle):
    """
    Clears the current mission commands on the drone.
    """
    print("Clearing existing mission...")
    cmds = vehicle.commands
    cmds.clear()
    vehicle.flush()
    print("Mission cleared.")


def add_mission(vehicle, waypoints):
    """
    Adds a mission to the drone, including the specified waypoints.
    """
    print("Adding mission with waypoints...")
    cmds = vehicle.commands
    cmds.clear()

    # Add a takeoff command
    cmds.add(Command(
        0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
        mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0,
        0, 0, 0, 0,
        0, 0, 10
    ))

    # Add waypoints
    for wp in waypoints:
        lat, lon, alt = wp
        print(f"Adding waypoint: {lat}, {lon}, {alt}")
        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,
            lat, lon, alt
        ))

    # Add a return to launch command
    cmds.add(Command(
        0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
        mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0,
        0, 0, 0, 0,
        0, 0, 0
    ))

    # Upload the mission to the drone
    cmds.upload()
    print("Mission uploaded successfully.")


def arm_and_takeoff(vehicle, target_altitude):
    """
    Arms the drone and takes off to the specified altitude.
    """
    print("Arming the drone...")
    while not vehicle.is_armable:
        print("Waiting for the vehicle to be armable...")
        time.sleep(1)

    vehicle.mode = VehicleMode("GUIDED")
    while vehicle.mode.name != "GUIDED":
        print("Waiting for GUIDED mode...")
        time.sleep(1)

    vehicle.armed = True
    while not vehicle.armed:
        print("Waiting for drone to arm...")
        time.sleep(1)

    print("Drone armed. Taking off...")
    vehicle.simple_takeoff(target_altitude)

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


def monitor_mission(vehicle, waypoint_count):
    """
    Monitors the mission progress and waits until the mission is complete.
    """
    print("Starting mission...")
    while True:
        next_wp = vehicle.commands.next
        print(f"Current waypoint: {next_wp}/{waypoint_count}")
        if next_wp == waypoint_count + 1:
            print("Mission completed.")
            break
        time.sleep(2)


def main():
    parser = argparse.ArgumentParser(description="Navigate through waypoints and land.")
    parser.add_argument("--connect", type=str, required=True, help="Connection string for the drone.")
    args = parser.parse_args()

    waypoints = [
        (32.79073078787342, -96.81124764100507, 10),
        (32.79137855322258, -96.81022315727263, 10),
        (32.79027341987208, -96.80935279595737, 10),
        (32.78970535452098, -96.81031360853898, 10)
    ]

    vehicle = connect_drone(args.connect)

    try:
        # Clear any existing mission
        clear_mission(vehicle)

        # Add new mission with waypoints
        add_mission(vehicle, waypoints)

        # Arm and takeoff
        arm_and_takeoff(vehicle, 10)

        # Switch to AUTO mode to execute the mission
        vehicle.mode = VehicleMode("AUTO")
        while vehicle.mode.name != "AUTO":
            print("Waiting for AUTO mode...")
            time.sleep(1)

        # Monitor mission progress
        monitor_mission(vehicle, len(waypoints))

        # Wait for the drone to return and land
        print("Returning to launch and landing...")
        while vehicle.armed:
            print("Waiting for drone to land...")
            time.sleep(1)

    finally:
        vehicle.close()
        print("Disconnected from the drone.")


if __name__ == "__main__":
    main()