Description: This script connects to a SITL (Software In The Loop) drone simulation, demonstrates takeoff, navigation to a waypoint, and a return-to-home sequence using DroneKit. The drone flies to a predefined waypoint at 32.747405, -97.083837 and returns to its original position. It uses a MAVLink command to trigger landing of the drone at the…

sitl_drone_waypoint_and_return.py

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

def connect_and_print_attributes(connection_string):
    print(f"Connecting to drone on {connection_string}...")

    # Connect to the vehicle
    try:
        vehicle = connect(connection_string, wait_ready=True)
        print("Connected to drone successfully!")

        # Print vehicle state
        print("Drone details:")
        print(f" - Mode: {vehicle.mode.name}")
        print(f" - Location: {vehicle.location.global_relative_frame}")
        print(f" - Attitude: {vehicle.attitude}")
        print(f" - Velocity: {vehicle.velocity}")

        return vehicle

    except Exception as e:
        print(f"An error occurred: {e}")
        return None

def arm_and_takeoff(vehicle, target_altitude):
    while not vehicle.is_armable:
        print("Waiting for vehicle to be armable...")
        time.sleep(1)
    print('Vehicle is now armable')

    # Ensure the vehicle is in GUIDED mode
    vehicle.mode = VehicleMode("GUIDED")
    while vehicle.mode.name != "GUIDED":
        print("Waiting for GUIDED mode...")
        time.sleep(1)
    print('Vehicle in GUIDED mode')

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

    print("Drone armed. Taking off...")

    # Take off to target altitude
    vehicle.simple_takeoff(target_altitude)

    # Wait until the drone reaches the 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 goto_waypoint(vehicle, latitude, longitude, altitude):
    """Fly the drone to a specific waypoint."""
    target_location = LocationGlobalRelative(latitude, longitude, altitude)
    print(f"Flying to waypoint: lat={latitude}, lon={longitude}, alt={altitude}")

    vehicle.simple_goto(target_location)

    while True:
        current_location = vehicle.location.global_relative_frame
        distance_to_target = get_distance_meters(target_location, current_location)
        print(f"Distance to target: {distance_to_target:.2f} meters")
        if distance_to_target <= 1.0:  # Consider reached within 1 meter
            print("Reached target waypoint.")
            break
        time.sleep(1)

def get_distance_meters(target_location, current_location):
    """Calculate the ground distance in meters between two locations."""
    dlat = target_location.lat - current_location.lat
    dlon = target_location.lon - current_location.lon
    return ((dlat**2 + dlon**2)**0.5) * 1.113195e5

def send_land_command(vehicle):
    print("Sending LAND command via MAVLink...")
    msg = vehicle.message_factory.command_long_encode(
        0, 0,    # target_system, target_component
        mavutil.mavlink.MAV_CMD_NAV_LAND, # command
        0,       # confirmation
        0, 0, 0, 0,  # params 1-4 (unused)
        0, 0,    # params 5-6 (latitude, longitude - set to 0 for current location)
        0        # param 7 (altitude - set to 0 for landing)
    )
    vehicle.send_mavlink(msg)
    vehicle.flush()

def return_to_home(vehicle):
    """Return the drone to the original position."""
    original_lat = 32.751200
    original_lon = -97.082800
    original_alt = 10  # Set original altitude to match takeoff altitude

    print("Returning to the original position...")
    goto_waypoint(vehicle, original_lat, original_lon, original_alt)

    while True:
        current_location = vehicle.location.global_relative_frame
        distance_to_home = get_distance_meters(LocationGlobalRelative(original_lat, original_lon, original_alt), current_location)
        print(f"Distance to home: {distance_to_home:.2f} meters")
        if distance_to_home <= 30.0:  # Trigger landing if within 30 meters of home
            print("Within 30 meters of home. Sending LAND command...")
            send_land_command(vehicle)
            break
        time.sleep(1)

def main():
    # Set up argument parser
    parser = argparse.ArgumentParser(description="Connect to a SITL drone using DroneKit.")
    parser.add_argument(
        "--connect", type=str, required=True, help="Connection string to connect to the drone (e.g., tcp:127.0.0.1:5760)"
    )

    args = parser.parse_args()

    connection_string = args.connect

    if not connection_string:
        import dronekit_sitl
        sitl = dronekit_sitl.start_default()
        connection_string = sitl.connection_string()

    # Connect to the drone and print attributes
    vehicle = connect_and_print_attributes(connection_string)

    # Arm, take off, and go to waypoints if connected
    if vehicle:
        arm_and_takeoff(vehicle, target_altitude=10)  # Example: Take off to 10 meters

        # Fly to the specific waypoint
        goto_waypoint(vehicle, latitude=32.747405, longitude=-97.083837, altitude=10)

        # Return to the original location
        return_to_home(vehicle)

        # Monitor altitude during landing
        while vehicle.armed:
            altitude = vehicle.location.global_relative_frame.alt
            print(f"Current altitude: {altitude:.2f} meters")
            if altitude <= 0.1:  # Consider landed if altitude is below 0.1 meters
                print("Landed successfully.")
                break
            time.sleep(1)

        vehicle.close()
        print("Disconnected from drone.")

if __name__ == "__main__":
    main()