NXP Hovergames 2 Challenge Submission: Deliver and Survery

# Author: Dean Dawson

import asyncio
from mavsdk import System
from mavsdk.offboard import (OffboardError, PositionNedYaw)

async def run():

    drone = System()
    await drone.connect(system_address="udp://:14540") # System address used for simulation testing

    print("Waiting for drone to connect...")           # Connect to the drone
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"Drone discovered with UUID: {state.uuid}")
            break

    print("Waiting for drone to have a global position estimate...")
    async for health in drone.telemetry.health():
        if health.is_global_position_ok:
            print("Global position estimate ok")
            break

    while 1:
        print("Please Choose an Option: 1 - Deliver , 2 - Survery")
        option = input()
        option = int(option)
        if(option == 1):
            break
        if(option == 2):
            break
        else:
            print("invalid option, Please try again")

    print("-- Arming")
    await drone.action.arm()

    print("-- Setting initial setpoint")
    await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, 0.0, 0.0))

    print("-- Starting offboard")
    try:
        await drone.offboard.start()
    except OffboardError as error:
        print(f"Starting offboard mode failed with error code: {error._result.result}")
        print("-- Disarming")
        await drone.action.disarm()
        return

    # When working with PositionNedYaw, format is in this way:
    #  PositionNedYaw(+ north / -south, +east / -west, -up / +down)

    height = -5.0         # Controls height of drone during survey / delivery

    if(option == 1): # delivery mode
        distance = 15.0             # distance of how far to move north for delivery
        deliveryLocation = 0.0      # other option for delivery, will sent drone directly to position

        print("Taking off to height of " + str(height) + " meters.")
        await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, height, 0.0))
        await asyncio.sleep(20)

        print("Going north " + str(distance) + " meters.")
        await drone.offboard.set_position_ned(PositionNedYaw(distance, 0.0, -5.0, 0.0))
        await asyncio.sleep(20)

        print("Landing at current position.")
        await drone.action.land()
        await asyncio.sleep(30)

    if(option == 2): #survey mode
        numOfCycles = 6       # Controls the number of survey cycles to complete
        northIncrement = 5.0  # Controls how far north the drone will travel with each cycle
        eastIncrement = -7.0  # Controls how far east/west the drone will travel with each cycle

        print("Taking off to height of " + str(height * -1) + " meters.")
        await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, height, 0.0))
        await asyncio.sleep(20)

        northDistanceCounter = 0.0   # keeps track of the distance traveled north     
        eastCounter = eastIncrement  # keeps track of direction, east/west
        inline = True                # keeps track of when to go north (only when inline with original postion)
        for i in range(numOfCycles):
            if i % 2 == 0:
                print("Going north " + str(northIncrement) + " meters.")
                northDistanceCounter += northIncrement
                if(inline == True):
                    await drone.offboard.set_position_ned(PositionNedYaw(northDistanceCounter, 0.0, height, 0.0))
                    await asyncio.sleep(15)
                else:
                    await drone.offboard.set_position_ned(PositionNedYaw(northDistanceCounter, eastCounter, height, 0.0))
                    await asyncio.sleep(15) 
                inline = not inline
            else:
                eastCounter = eastCounter * -1
                if(eastCounter > 0):
                    print("Going east " + str(eastIncrement * -1) + " meters.")
                    await drone.offboard.set_position_ned(PositionNedYaw(northDistanceCounter, eastCounter, height, 0.0))
                    await asyncio.sleep(15)
                else:
                    print("Going west " + str(eastIncrement * -1) + " meters.")
                    await drone.offboard.set_position_ned(PositionNedYaw(northDistanceCounter, 0.0, height, 0.0))
                    await asyncio.sleep(15)
        
        print("Landing at current position.")
        await drone.action.land()
        await asyncio.sleep(30)

        # The Code below (commented out) is the manual pattern for survey
            
        # print("-- Go 5m North, 0m East, 0m up within local coordinate system")
        # await drone.offboard.set_position_ned(PositionNedYaw(5.0, 0.0, -5.0, 0.0))
        # await asyncio.sleep(20)

        # print("-- Go 0m North, 7m East, 0m up within local coordinate system")
        # await drone.offboard.set_position_ned(PositionNedYaw(5.0, 7.0, -5.0, 0.0))
        # await asyncio.sleep(20)

        # print("-- Go 5m North, 0m East, 0m up within local coordinate system")
        # await drone.offboard.set_position_ned(PositionNedYaw(10.0, 7.0, -5.0, 0.0))
        # await asyncio.sleep(20)

        # print("-- Go 0m North, 7m West, 0m up within local coordinate system")
        # await drone.offboard.set_position_ned(PositionNedYaw(10.0, 0.0, -5.0, 0.0))
        # await asyncio.sleep(20)

    print("-- Stopping offboard")
    try:
        await drone.offboard.stop()
    except OffboardError as error:
        print(f"Stopping offboard mode failed with error code: {error._result.result}")

   
if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    loop.run_until_complete(run())