def __init__(self, loop, server_address, server_port, device, baudrate):
self._loop = loop
self._drone = System(mavsdk_server_address=server_address,
port=server_port)
self._loop.run_until_complete(
self._drone.connect(system_address="serial://" + device + ":" +
str(baudrate)))
async def run():
drone = System()
await drone.connect("tcp://localhost:4560")
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
# await drone.param.set_param_float("MIS_DIST_WPS", 5000)
print_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_plan_cor = drone.mission.import_qgroundcontrol_mission(
"/Users/rachayitagiri/helping-hands/traffic_circle.plan")
mission_plan = await mission_plan_cor
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def run():
""" Does Offboard control using position NED coordinates. """
drone = System()
await drone.connect(system_address=aa['address'])
print("-- Arming")
await drone.action.arm()
await drone.action.set_takeoff_altitude(5.0)
await drone.action.set_maximum_speed(0.5)
print("--Taking off")
await drone.action.takeoff()
await asyncio.sleep(8)
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
print("-- Go 0m North, 0m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -5.0, 0.0))
await asyncio.sleep(5)
print("-- Go 5m North, 0m East, -5m Down within local coordinate system, turn to face East")
await drone.offboard.set_position_ned(PositionNedYaw(10.0, 10.0, -5.0, 0.0))
await asyncio.sleep(15)
# print("-- Go 5m North, 5m East, -5m Down within local coordinate system")
# await drone.offboard.set_position_ned(PositionNedYaw(5.0, 5.0, -5.0, 0.0))
# await asyncio.sleep(5)
# print("-- Go 5m North, 5m East, -5m Down within local coordinate system")
# await drone.offboard.set_position_ned(PositionNedYaw(0.0, 5.0, -5.0, 0.0))
# await asyncio.sleep(5)
print("-- Go 0m North, 0m East, -5m Down within local coordinate system")
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -5.0, 0.0))
await asyncio.sleep(15)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
await asyncio.sleep(10)
print("--Landing")
await drone.action.land()
await asyncio.sleep(12)
print("--Disarming")
await drone.action.disarm()
except OffboardError as error:
print(f"Stopping offboard mode failed with error code: {error._result.result}")
async def take_off():
drone = System()
await drone.connect(system_address="udp://:14540")
print("Waiting for drone...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
print("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(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 errored with code {error._result.result}")
print("-- Disarming")
await drone.action.disarm()
return
print("-- Taking off")
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, -0.5, 0.0))
await asyncio.sleep(3)
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, 0.0, 0.0))
async def run():
drone = System()
await drone.connect(system_address="serial:///dev/ttyUSB0")
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 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
print("Fetching amsl altitude at home location....")
async for terrain_info in drone.telemetry.home():
absolute_altitude = terrain_info.absolute_altitude_m
break
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(1)
flying_alt = absolute_altitude + 2.5 #To fly drone 3m above the ground plane
while True:
#goto_location() takes Absolute MSL altitude
await drone.action.goto_location(target_gps.latitude,
target_gps.longitude, flying_alt, 0)
async def prepare_before_run():
current_location = "E"
fly_to_new_location = False
location_E = [59.347372137351705, 18.073428604055536]
location_Q = [59.349954061446454, 18.067289782044295]
location_M = [59.35337996235275, 18.065071080920088]
drone = System()
await drone.connect()
print("Waiting for 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
print("Fetching amsl altitude at home location....")
async for terrain_info in drone.telemetry.home():
absolute_altitude = terrain_info.absolute_altitude_m
break
flying_alt = absolute_altitude + 100
repeat = True
await run(drone, current_location, fly_to_new_location, location_E, location_Q, location_M, repeat,
flying_alt)
def __init__(self, drone_udp, mass, radius, height, num_rotors,
thrust_curve):
"""
Constructor of the UAV class.
Parameters
----------
drone_udp : str
UDP address of the PX4 autopilot.
mass : str
Mass of the drone.
radius : str
Radius of the vehicle.
height : str
Height of the drone.
num_rotors : str
Number of rotors of the drone.
thrust_curve : str
Thrust curve of the vehicle.
"""
self.ekf = EKF()
self.act = ACTUATORS()
self.info = DRONE_INFO()
self.info.drone_udp = "udp://:" + drone_udp
self.info.mass = float(mass)
self.info.radius = float(radius)
self.info.height = float(height)
self.info.num_rotors = int(num_rotors)
self.info.thrust_curve = thrust_curve
self.system = System(port=50050 + int(drone_udp[-3:]))
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 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
print("Fetching amsl altitude at home location....")
async for terrain_info in drone.telemetry.home():
absolute_altitude = terrain_info.absolute_altitude_m
break
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(1)
flying_alt = absolute_altitude + 20.0 #To fly drone 20m above the ground plane
#goto_location() takes Absolute MSL altitude
await drone.action.goto_location(47.399386, 8.535245, flying_alt, 0)
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
asyncio.ensure_future(print_camera_mode(drone))
asyncio.ensure_future(print_camera_status(drone))
print("Setting mode to 'PHOTO'")
try:
await drone.camera.set_mode(CameraMode.PHOTO)
except CameraError as error:
print(f"Setting mode failed with error code: {error._result.result}")
await asyncio.sleep(2)
print("Taking a photo")
try:
await drone.camera.take_photo()
except CameraError as error:
print(f"Couldn't take photo: {error._result.result}")
# Shut down the running coroutines (here 'print_camera_mode()' and
# 'print_camera_status()')
await asyncio.get_event_loop().shutdown_asyncgens()
async def main():
# Create a drone object and connect
drone = System()
await drone.connect(system_address="udp://:14540") # Default port for PX4 SITL simulation
print("Waiting for drone to connect")
async for state in drone.core.connection_state(): # Check for connection
if state.is_connected:
print("Connected to drone with UUID: " + str({state.uuid}))
break
# Calculates points for the drone to visit in its search
mission = perimeter(47.398041, 8.544287, 80) # Outer rectangle
mission.extend(perimeter(47.398041, 8.544287, 40)[1:]) # Inner rectangle
# Starts the mission
await start_mission_RTL(drone, mission)
# Provides feedbcak about the drone's mission progress
async for mission_progress in drone.mission.mission_progress():
if mission_progress.current_item_index == 1:
print("Reached search area")
if mission_progress.current_item_index == mission_progress.mission_count:
print("Search completed, returning to launch")
return
async def run():
drone = System()
await drone.connect(system_address="udp://:14550")
print("Waiting for drone...")
async for state in drone.core.connection_state():
if state.is_connected:
print(f"Drone discovered with UUID: {state.uuid}")
break
home = await get_home_pos(drone)
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(10)
await drone.offboard.set_velocity_ned(offboard.VelocityNedYaw(0, 0, 0, 0))
await drone.offboard.start()
v = 5
await drone.offboard.set_velocity_ned(offboard.VelocityNedYaw(v, 0, 0, 0))
print(f"-- Moving forward at {v} m/s")
await asyncio.ensure_future(check_dist(drone, home, 5e-07))
await drone.offboard.stop()
await drone.action.return_to_launch()
print("-- Returning Home")
async for landed in drone.telemetry.landed_state():
if landed == LandedState.ON_GROUND:
print("-- Landed")
break
await asyncio.sleep(10)
await drone.action.disarm()
async def main(host, port):
drone = System()
await drone.connect(system_address="udp://:14555")
# await drone.connect(system_address="/dev/ttyUSB0")
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 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
print("-- Setting initial setpoint")
await drone.offboard.set_attitude(Attitude(0.0, 0.0, 0.0, 0.0))
print("-- Setting takeoff altitude to 5m")
# await drone.action.set_takeoff_altitude(
print(await drone.action.get_takeoff_altitude())
print("Ready to accept incoming connections")
server = await asyncio.start_server(lambda r, w: echo_server(r, w, drone),
host, port)
async with server:
await server.serve_forever()
class SimulatedDrone:
drone = System()
s = Simulation()
s.on()
s.run_sim()
async def takeoff(self):
await self.drone.connect(system_address="udp://:14540")
drone = System()
print("-- Arming")
await self.drone.action.arm()
print("-- Setting initial setpoint")
await self.drone.offboard.set_position_ned(
PositionNedYaw(0.0, 0.0, 0.0, 0.0))
try:
await self.drone.offboard.start()
except OffboardError as error:
print(
f"Starting offboard mode failed with error code: {error._result.result}"
)
print("-- Disarming")
await self.drone.action.disarm()
return
async def controlToMakeQuarter(self):
await asyncio.sleep(5)
height = True
if (height):
print("less than 5m")
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, -1.0, 0.0))
await asyncio.sleep(1)
height = False
i = 0
while (i < self.s.cp.stack.buf.__len__()):
if self.s.cp.stack.pop_from_stack()[0] > 7:
print(self.s.cp.stack.pop_from_stack()[0])
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(4.0, 0.0, 0.0, 0.0))
print("nothing in front")
await asyncio.sleep(3)
else:
await self.drone.offboard.set_velocity_body(
VelocityBodyYawspeed(0.0, 0.0, 0.0, 30.0))
print("turn left")
await asyncio.sleep(3)
i += 1
print("-- Stopping offboard")
try:
await self.drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
async def run():
""" Does Offboard control using attitude commands. """
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("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_attitude(Attitude(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
async def run():
uav = System()
await uav.connect(system_address="udp://:14540") #Connects to the UAV
print("Establishing Connection...")
#Check CONNECTION & if there is a positive connection Feedback = CONNECTED
async for state in uav.core.connection_state():
if state.is_connected:
print("UAV target UUID: {state.uuid}"
) #Prints the UUID of the UAV to which the system connected
break
print("Establishing GPS lock on UAV..")
#Checks the gps Connection via telemetry health command
async for health in uav.telemetry.health():
if health.is_global_position_ok:
print("Established GPS lock...") #GPS health approved
break
print("Arming UAV")
await uav.action.arm()
print("Taking Off")
await uav.action.takeoff()
await asyncio.sleep(10)
print("LANDING")
await uav.action.land()
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 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
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(5)
print("-- Landing")
await drone.action.land()
async def run():
#drone = System(mavsdk_server_address="localhost", port=50043)
drone = System()
await drone.connect(system_address="udp://:14544")
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 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
print("Fetching home location coordinates...")
async for terrain_info in drone.telemetry.home():
home_lat = terrain_info.latitude_deg
home_lon = terrain_info.longitude_deg
home_abs_alt=terrain_info.absolute_altitude_m
print(home_lat,home_lon,home_abs_alt)
break
print("-- Arming")
await drone.action.arm()
await asyncio.sleep(8)
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
# Start parallel tasks
print_flight_mode_task = asyncio.ensure_future(print_flight_mode(drone))
running_tasks = [print_flight_mode_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
asyncio.ensure_future(print_mission_progress(drone))
#asyncio.ensure_future(print_position(drone))
waypoint1 = Waypoint(47.398039859999997, 8.5455725400000002)
waypoint2 = Waypoint(47.398036222362471, 8.5450146439425509)
waypoint3 = Waypoint(47.397825620791885, 8.5450092830163271)
flight_altitude = 5
flight_speed = 10
mission_items = []
mission_items.append(
MissionItem(waypoint1.latitude, waypoint1.longitude,
flight_altitude, flight_speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_items.append(
MissionItem(waypoint2.latitude, waypoint2.longitude,
flight_altitude, flight_speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_items.append(
MissionItem(waypoint3.latitude, waypoint3.longitude,
flight_altitude, flight_speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_items)
print("-- Arming")
await drone.action.arm()
print("-- Mission")
await drone.mission.start_mission()
# Wait until the drone is landed (instead of exiting after 'land' is sent)
await termination_task
async def print_flight_mode(yolo, helipad_edge, image_width, image_height):
drone = System()
await drone.connect(system_address=aa['address'])
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
async for flight_mode in drone.telemetry.flight_mode():
print("FlightMode:{}|||||||||".format(flight_mode))
if str(flight_mode) == "HOLD":
await asyncio.sleep(10)
break
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
with picamera.PiCamera() as camera:
camera.resolution = (640, 480)
camera.start_preview()
count = 0
for image in image_generator(camera):
print('image' + str(count) + '.jpg')
image.save('./video/test' + str(count) + '.jpg', 'jpeg')
# image = Image.open('./video/outputframe2.jpg')
count += 1
result = detect_img(yolo, image)
result = calculate_distance(result, helipad_edge, image_width,
image_height)
if result is not None:
print(result)
print(
"-- Go 0m North, 10m East, 0m Down within local coordinate system, turn to face South"
)
await drone.offboard.set_position_ned(
PositionNedYaw(result['actual_delta_x'],
result['actual_delta_y'], 0.0, 0.0))
await asyncio.sleep(10)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
await drone.action.land()
await asyncio.sleep(5)
break
print(result)
async def run():
""" Does Offboard control using velocity NED coordinates. """
drone = System()
await drone.connect(system_address="udp://:14540")
print("-- Arming")
await drone.action.arm()
print("-- Setting initial setpoint")
await drone.offboard.set_velocity_ned(VelocityNedYaw(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
print("-- Go up 2 m/s")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, -2.0, 0.0))
await asyncio.sleep(4)
print("-- Go North 2 m/s, turn to face East")
await drone.offboard.set_velocity_ned(VelocityNedYaw(2.0, 0.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Go South 2 m/s, turn to face West")
await drone.offboard.set_velocity_ned(VelocityNedYaw(
-2.0, 0.0, 0.0, 270.0))
await asyncio.sleep(4)
print("-- Go West 2 m/s, turn to face East")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, -2.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Go East 2 m/s")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 2.0, 0.0, 90.0))
await asyncio.sleep(4)
print("-- Turn to face South")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 0.0, 180.0))
await asyncio.sleep(2)
print("-- Go down 1 m/s, turn to face North")
await drone.offboard.set_velocity_ned(VelocityNedYaw(0.0, 0.0, 1.0, 0.0))
await asyncio.sleep(4)
print("-- Stopping offboard")
try:
await drone.offboard.stop()
except OffboardError as error:
print(
f"Stopping offboard mode failed with error code: {error._result.result}"
)
async def run():
drone = System()
# await drone.connect("serial:///dev/tty.usbmodem01:57600")
await drone.connect(system_address="udp://:14540")
destinations = []
proceed = "Y"
while (proceed == "Y" or proceed == "y"):
situation_type = get_situation_category()
drone_type = get_drone_type_from_choice(situation_type)
lat, lng = get_destination_latitude_longitude()
destinations.append([situation_type, drone_type, [lat, lng]])
proceed = input("\nDo you need to add more destinations? (Y/N): ")
# print("\nDispatching the following drones for the corresponding situations:")
# for dest in destinations:
# print(dest)
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_mission_progress_task = asyncio.ensure_future(
print_mission_progress(drone))
running_tasks = [print_mission_progress_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
mission_items = []
print(
"\nDispatching the following drones for the corresponding situations:")
for destination in destinations:
print(destination)
lat, lng = destination[2][0], destination[2][1]
mission_items.append(
MissionItem(lat, lng, 25, 10, True, float('nan'), float('nan'),
MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
mission_plan = MissionPlan(mission_items)
await drone.mission.set_return_to_launch_after_mission(True)
# print("-- Clearing previous missions")
# await drone.mission.clear_mission()
print("-- Uploading mission")
await drone.mission.upload_mission(mission_plan)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
async def power_up(self):
drone = System()
await drone.connect(system_address="udp://:14540") # Connects to FCPU (Raspberry Pi)
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
async def run():
# Init the drone
drone = System()
await drone.connect(system_address="udp://:14540")
# Start the tasks
asyncio.ensure_future(print_position(drone))
asyncio.ensure_future(print_euler(drone))
async def run():
# Init the drone
drone = System()
#await drone.connect(system_address="udp://:14540")
await drone.connect()
# Start the tasks
asyncio.ensure_future(print_transponders(drone))
async def run():
drone = System()
await drone.connect(system_address="udp://:14540")
print("-- Starting gyro calibration")
async for progress_data in drone.calibration.calibrate_gyro():
print(progress_data)
async def run():
"""
This is the "main" function.
It first creates the drone object and initializes it.
Then it registers tasks to be run in parallel (one can think of them as
threads):
- print_altitude: print the altitude
- print_flight_mode: print the flight mode
- observe_is_in_air: this monitors the flight mode and returns when the
drone has been in air and is back on the ground.
Finally, it goes to the actual works: arm the drone, initiate a takeoff
and finally land.
Note that "observe_is_in_air" is not necessary, but it ensures that the
script waits until the drone is actually landed, so that we receive
feedback during the landing as well.
"""
#Start SITL if no connection string specified
connection_string = ''
connection_string = 'udp:0.0.0.0:14550'
sitl = None
if not connection_string:
import dronekit_sitl
sitl = dronekit_sitl.start_default()
connection_string = sitl.connection_string()
# Init the drone
drone = System()
await drone.connect(connection_string)
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
# Start parallel tasks
print_altitude_task = asyncio.ensure_future(print_altitude(drone))
print_flight_mode_task = asyncio.ensure_future(print_flight_mode(drone))
running_tasks = [print_altitude_task, print_flight_mode_task]
termination_task = asyncio.ensure_future(
observe_is_in_air(drone, running_tasks))
# Execute the maneuvers
print("-- Arming")
await drone.action.arm()
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(5)
print("-- Landing")
await drone.action.land()
# Wait until the drone is landed (instead of exiting after 'land' is sent)
await termination_task
async def run():
global euler_angles
tracking = False # assume that target is not in frame when initializing
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
async for is_armed in drone.telemetry.armed():
print("Is_armed:", is_armed)
if not is_armed:
print("-- Arming")
await drone.action.arm()
break
print("-- Setting initial setpoint")
await drone.offboard.set_velocity_body(
VelocityBodyYawspeed(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
await drone.offboard.set_position_ned(PositionNedYaw(0.0, 0.0, -10.0, 90.0))
async for drone_position in drone.telemetry.odometry():
if (math.fabs(drone_position.position_body.x_m) < 0.2 and
math.fabs(drone_position.position_body.y_m) < 0.2 and
math.fabs(drone_position.position_body.z_m + 10) < 0.2):
break
for i in range(3, 0, -1):
print("Switching to IBVS in " + str(i) + "..")
await asyncio.sleep(1)
while True:
await asyncio.sleep(1 / 30.0)
async for euler_angles in drone.telemetry.attitude_euler():
break
v_xr, v_yr, yawrate, v_zr = getsetpoints()
print("vx: " + str(v_xr) + " vy: " + str(v_yr) + " vz: " + str(v_zr) + " yawrate: " + str(yawrate))
await drone.offboard.set_velocity_body(
# VelocityBodyYawspeed(v_xr, v_yr, v_zr, yawrate)
VelocityBodyYawspeed(v_xr, v_yr, v_zr, yawrate)
)
async def manual_controls():
"""Main function to connect to the drone and input manual controls"""
# Connect to the Simulation
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 ok")
break
# set the manual control input after arming
await drone.manual_control.set_manual_control_input(
float(0), float(0), float(0.5), float(0))
# Arming the drone
print("-- Arming")
await drone.action.arm()
# Takeoff the vehicle
print("-- Taking off")
await drone.action.takeoff()
await asyncio.sleep(5)
# set the manual control input after arming
await drone.manual_control.set_manual_control_input(
float(0), float(0), float(0.5), float(0))
# start manual control
print("-- Starting manual control")
await drone.manual_control.start_position_control()
while True:
# grabs a random input from the test list
# WARNING - your simulation vehicle may crash if its unlucky enough
input_index = random.randint(0, len(manual_inputs) - 1)
input_list = manual_inputs[input_index]
# get current state of roll axis (between -1 and 1)
roll = float(input_list[0])
# get current state of pitch axis (between -1 and 1)
pitch = float(input_list[1])
# get current state of throttle axis (between -1 and 1, but between 0 and 1 is expected)
throttle = float(input_list[2])
# get current state of yaw axis (between -1 and 1)
yaw = float(input_list[3])
await drone.manual_control.set_manual_control_input(
roll, pitch, throttle, yaw)
await asyncio.sleep(0.1)
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
asyncio.ensure_future(print_mission_progress(drone))
termination_task = asyncio.ensure_future(observe_is_in_air(drone))
mission_items = []
mission_items.append(MissionItem(47.398039859999997,
8.5455725400000002,
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(47.398036222362471,
8.5450146439425509,
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
mission_items.append(MissionItem(47.397825620791885,
8.5450092830163271,
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
await drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await drone.mission.upload_mission(mission_items)
print("-- Arming")
await drone.action.arm()
print("-- Starting mission")
await drone.mission.start_mission()
await termination_task
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
song_elements = []
song_elements.append(SongElement.DURATION_4);
song_elements.append(SongElement.NOTE_G);
song_elements.append(SongElement.NOTE_A);
song_elements.append(SongElement.NOTE_B);
song_elements.append(SongElement.FLAT);
song_elements.append(SongElement.OCTAVE_UP);
song_elements.append(SongElement.DURATION_1);
song_elements.append(SongElement.NOTE_E);
song_elements.append(SongElement.FLAT);
song_elements.append(SongElement.OCTAVE_DOWN);
song_elements.append(SongElement.DURATION_4);
song_elements.append(SongElement.NOTE_PAUSE);
song_elements.append(SongElement.NOTE_F);
song_elements.append(SongElement.NOTE_G);
song_elements.append(SongElement.NOTE_A);
song_elements.append(SongElement.OCTAVE_UP);
song_elements.append(SongElement.DURATION_2);
song_elements.append(SongElement.NOTE_D);
song_elements.append(SongElement.NOTE_D);
song_elements.append(SongElement.OCTAVE_DOWN);
song_elements.append(SongElement.DURATION_4);
song_elements.append(SongElement.NOTE_PAUSE);
song_elements.append(SongElement.NOTE_E);
song_elements.append(SongElement.FLAT);
song_elements.append(SongElement.NOTE_F);
song_elements.append(SongElement.NOTE_G);
song_elements.append(SongElement.OCTAVE_UP);
song_elements.append(SongElement.DURATION_1);
song_elements.append(SongElement.NOTE_C);
song_elements.append(SongElement.OCTAVE_DOWN);
song_elements.append(SongElement.DURATION_4);
song_elements.append(SongElement.NOTE_PAUSE);
song_elements.append(SongElement.NOTE_A);
song_elements.append(SongElement.OCTAVE_UP);
song_elements.append(SongElement.NOTE_C);
song_elements.append(SongElement.OCTAVE_DOWN);
song_elements.append(SongElement.NOTE_B);
song_elements.append(SongElement.FLAT);
song_elements.append(SongElement.DURATION_2);
song_elements.append(SongElement.NOTE_G);
tune_description = TuneDescription(song_elements, 200)
await drone.tune.play_tune(tune_description)
print("Tune played")
