async def main_loop():
drone = System()
await connect_drone(drone)
print("Initializing tasks")
# Start parallel tasks
print_altitude_task = asyncio.ensure_future(print_altitude(drone))
print_flight_mode_task = asyncio.ensure_future(print_flight_mode(drone))
print_battery_task = asyncio.ensure_future(print_battery(drone))
print_GPSfix_task = asyncio.ensure_future(print_gpsfix(drone))
running_tasks = [print_altitude_task, print_flight_mode_task, print_battery_task, print_GPSfix_task]
#running_tasks = [print_altitude_task, print_flight_mode_task, print_health_task, print_coords_task]
termination_task = asyncio.ensure_future(observe_is_in_air(drone, running_tasks))
# Execute the maneuvers
print("-- Arming")
await drone.action.arm()
#Follow me Mode requires some configuration to be done before starting the mode
conf = Config(default_height, follow_distance, direction, responsiveness)
await drone.follow_me.set_config(conf)
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(10)
print("-- Starting Follow Me Mode")
await drone.follow_me.start()
await asyncio.sleep(10)
for latitude,longitude in fake_location:
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print (f"-- Following Target: {latitude},{longitude}")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(2)
await asyncio.sleep(5)
print ("-- Stopping Follow Me Mode")
await drone.follow_me.stop()
await asyncio.sleep(100)
await drone.action.home()
print("-- Landing")
await drone.action.land()
async def fly_drone():
drone = System()
await drone.connect(system_address="udp://:14540")
#This waits till a mavlink based drone is connected
async for state in drone.core.connection_state():
if state.is_connected:
print(f"-- Connected to drone with UUID: {state.uuid}")
break
#Checking if Global Position Estimate is ok
async for global_lock in drone.telemetry.health():
if global_lock.is_global_position_ok:
print("-- Global position state is good enough for flying.")
break
#Arming the drone
print("-- Arming")
await drone.action.arm()
#Follow me Mode requires some configuration to be done before starting the mode
conf = Config(default_height, follow_distance, direction, responsiveness)
await drone.follow_me.set_config(conf)
print("-- Taking Off")
await drone.action.takeoff()
await asyncio.sleep(8)
print("-- Starting Follow Me Mode")
await drone.follow_me.start()
await asyncio.sleep(8)
#This for loop provides fake coordinates from the fake_location list for the follow me mode to work
#In a simulator it won't make much sense though
for latitude, longitude in fake_location:
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print("-- Following Target")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(2)
#Stopping the follow me mode
print("-- Stopping Follow Me Mode")
await drone.follow_me.stop()
await asyncio.sleep(5)
print("-- Landing")
await drone.action.land()
async def main_loop(asyncQueue):
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print("Waiting for global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate OK")
break
window = initGUI()
dInfo = initInfoPanels(DRONE_INFO_TAGS,
'-- Drone Health + Positional Info --', window, 0)
textLabel(window, "\n---------------\n", 0, -1)
rInfo = initInfoPanels(ROV_INFO_TAGS, '-- ROV Info --', window, 0)
command = commandDeque(window, 1)
transfer_task = asyncio.ensure_future(parseCommand(command, asyncQueue))
GUI_task = asyncio.ensure_future(updateGUI(command, window))
dataLogging_task = asyncio.ensure_future(
outputToDataLog([dInfo, rInfo], command, DATALOG_FILENAME,
DATALOG_OUTPUT_MODE))
#print_health_task = asyncio.ensure_future(update_drone_info(drone, dInfo))
updateInfoTasks = update_drone_info(drone, dInfo)
#print_coords_task = asyncio.ensure_future(tcp_echo_client(asyncio.get_event_loop()))
################
try:
print("-- Arming")
await drone.action.arm()
except:
print("-- Already Armed")
# Execute the maneuvers
try:
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(10)
except:
print("-- Already in air")
#Follow me Mode requires some configuration to be done before starting the mode
conf = Config(default_height, follow_distance, direction, responsiveness)
await drone.follow_me.set_config(conf)
print("-- Starting Follow Me Mode")
await drone.follow_me.start()
await asyncio.sleep(10)
'''
while missionQueue.qsize() < 2:
print("Waiting for target coords...")
await asyncio.sleep(20)
while missionQueue.qsize() != 0:
[latitude, longitude] = missionQueue.get()
print(f"-- {missionQueue.qsize()} target coords left")
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print (f"-- Following Target: {latitude},{longitude}")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(10)
'''
for latitude, longitude in fake_location:
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print(f"-- Following Target: {latitude},{longitude}")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(2)
await asyncio.sleep(5)
print("-- Stopping Follow Me Mode")
await drone.follow_me.stop()
await asyncio.sleep(5)
print("-- Landing")
await drone.action.land()
while 1:
data = None
if command.count() != 0:
data = command.popTop()
else:
command.clearCurrent()
if data == "quit":
print("----- quitting -----")
break
asyncio.ensure_future(processCommand(data))
#print(data)
await asyncio.sleep(2)
async def processCommand(drone, deque):
while 1:
data = None
if deque.count() != 0:
command = deque.popTop()
args = command.split(",")
if args[0] == COMMANDS[0]:
try:
await drone.action.arm()
await asyncio.sleep(BASE_ACTION_DELAY)
except:
pass
elif args[0] == COMMANDS[1]:
try:
await drone.action.takeoff()
await asyncio.sleep(BASE_ACTION_DELAY)
except:
pass
elif args[0] == COMMANDS[2]:
try:
await drone.action.land()
await asyncio.sleep(BASE_ACTION_DELAY)
except:
pass
elif args[0] == COMMANDS[3]:
try:
await drone.action.disarm()
await asyncio.sleep(BASE_ACTION_DELAY)
except:
pass
elif args[0] == COMMANDS[4]:
try:
await drone.action.kill()
except:
pass
elif args[0] == COMMANDS[5]: #goto
try:
lat = float(args[1])
lon = float(args[2])
absolute_altitude = 0.0
async for terrain_info in drone.telemetry.home():
absolute_altitude = terrain_info.absolute_altitude_m
break
await drone.action.goto_location(
lat, lon, absolute_altitude + DEFAULT_HEIGHT, 0)
await asyncio.sleep(TIME_PER_COORD)
except:
print(f"Bad Request: {args}")
pass
elif args[0] == COMMANDS[6]: #hold
try:
await asyncio.sleep(float(args[1]))
except:
print(f"Bad Request: {args}")
pass
elif args[0] == COMMANDS[7]: #start follow
try:
conf = Config(DEFAULT_HEIGHT, FOLLOW_DISTANCE,
FOLLOW_DIRECTION, FOLLOW_RESPONSIVENESS)
await drone.follow_me.set_config(conf)
await drone.follow_me.start()
except:
print(f"Bad Request: {args}")
pass
elif args[0] == COMMANDS[8]: #follow target
try:
target = TargetLocation(float(args[1]), float(args[2]), 0,
0, 0, 0)
await drone.follow_me.set_target_location(target)
except:
print(f"Bad Request: {args}")
pass
elif args[0] == COMMANDS[9]: #stop follow
try:
await drone.follow_me.stop()
except:
pass
elif args[0] == "quit":
print("-- quitting --")
break
else:
print(f"?: {args}")
await asyncio.sleep(10)
else:
deque.clearCurrent()
await asyncio.sleep(DEQUE_RESPOND_PERIOD)
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone to connect...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print("Waiting for global position estimate...")
async for health in drone.telemetry.health():
if health.is_global_position_ok:
print("Global position estimate ok")
break
# Start parallel tasks
print_altitude_task = asyncio.ensure_future(print_altitude(drone))
print_flight_mode_task = asyncio.ensure_future(print_flight_mode(drone))
print_health_task = asyncio.ensure_future(print_health(drone))
#print_coords_task = asyncio.ensure_future(tcp_echo_client(asyncio.get_event_loop()))
running_tasks = [
print_altitude_task, print_flight_mode_task, print_health_task
]
#running_tasks = [print_altitude_task, print_flight_mode_task, print_health_task, print_coords_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
try:
print("-- Arming")
await drone.action.arm()
except:
print("-- Already Armed")
# Execute the maneuvers
try:
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(10)
except:
print("-- Already in air")
#Follow me Mode requires some configuration to be done before starting the mode
conf = Config(default_height, follow_distance, direction, responsiveness)
await drone.follow_me.set_config(conf)
print("-- Starting Follow Me Mode")
await drone.follow_me.start()
await asyncio.sleep(10)
'''
while missionQueue.qsize() < 2:
print("Waiting for target coords...")
await asyncio.sleep(20)
while missionQueue.qsize() != 0:
[latitude, longitude] = missionQueue.get()
print(f"-- {missionQueue.qsize()} target coords left")
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print (f"-- Following Target: {latitude},{longitude}")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(10)
'''
for latitude, longitude in fake_location:
target = TargetLocation(latitude, longitude, 0, 0, 0, 0)
print(f"-- Following Target: {latitude},{longitude}")
await drone.follow_me.set_target_location(target)
await asyncio.sleep(2)
await asyncio.sleep(5)
print("-- Stopping Follow Me Mode")
await drone.follow_me.stop()
await asyncio.sleep(5)
print("-- Landing")
await drone.action.land()