def perimeter(lat, lon, radius):
x, y, z = lat_lon_to_coords(lat, lon)
radius /= 1000 # Converts radius from metres to kilometres
alt = 25 # Altitude of drone
corners = []
# Go to centre of search area
corners.append(MissionItem(lat, lon, alt, 0, True, float('nan'), float('nan'), MissionItem.CameraAction.NONE, float('nan'), float('nan')))
# Calculate latitude and longitude of corner 1
lat, lon = coords_to_lat_lon(x, y+radius, z+radius)
corners.append(MissionItem(lat, lon, alt, 0, True, float('nan'), float('nan'), MissionItem.CameraAction.NONE, float('nan'), float('nan')))
# Calculate latitude and longitude of corner 2
lat, lon = coords_to_lat_lon(x, y+radius, z-radius)
corners.append(MissionItem(lat, lon, alt, 0, True, float('nan'), float('nan'), MissionItem.CameraAction.NONE, float('nan'), float('nan')))
# Calculate latitude and longitude of corner 3
lat, lon = coords_to_lat_lon(x, y-radius, z-radius)
corners.append(MissionItem(lat, lon, alt, 0, True, float('nan'), float('nan'), MissionItem.CameraAction.NONE, float('nan'), float('nan')))
# Calculate latitude and longitude of corner 4
lat, lon = coords_to_lat_lon(x, y-radius, z+radius)
corners.append(MissionItem(lat, lon, alt, 0, True, float('nan'), float('nan'), MissionItem.CameraAction.NONE, float('nan'), float('nan')))
# Complete search pattern by returning to corner 1
corners.append(corners[1])
return corners
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 run():
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()
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():
mission_items = []
for p in points_geo:
mission_items.append(MissionItem(p[0],
p[1],
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()
async def mission(self):
"""test to see if drone mission works"""
await self.connect()
asyncio.ensure_future(self.print_mission_progress())
termination_task = asyncio.ensure_future(self.observe_is_in_air())
mission_items = []
mission_items.append(MissionItem(38.1446916666667,
-76.4279944444445,
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
await self.drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await self.drone.mission.upload_mission(mission_items)
print("-- Arming")
await self.drone.action.arm()
print("-- Starting mission")
await self.drone.mission.start_mission()
await termination_task
async def initialize_mission(drone, MISSION_LAT, MISSION_LON, ACTION_HEIGHT):
""" intialize the mission items for the mission """
mission_items = []
mission_items.append(
MissionItem(MISSION_LAT, MISSION_LON,
MISSION_HEIGHT, MISSION_SPEED, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE, float('nan'),
float('nan')))
# ADD ACTION (EXTINGUISHING) POINT AS A MISSION ITEM
mission_items.append(
MissionItem(MISSION_LAT, MISSION_LON, ACTION_HEIGHT, 0, False,
float('nan'), float('nan'), MissionItem.CameraAction.NONE,
ACTION_TIME_SECS, float('nan')))
for mission in mission_items:
print(mission)
await drone.mission.set_return_to_launch_after_mission(True)
return mission_items
async def _update_mission(drone, lat, lng, wait=5):
mission_items = []
mission_items.append(
MissionItem(lat, lng, 10, 60, True, 0, 0,
MissionItem.CameraAction.TAKE_PHOTO, wait, float('nan')))
await drone.mission.set_return_to_launch_after_mission(True)
print(f"-- UPDATE mission")
await drone.mission.upload_mission(mission_items)
print(f"-- START mission after UPDATE")
await drone.mission.start_mission()
async def load_mission(self, mission_id, interop_client_bridge):
"""get mission from the interop server and send it to drone."""
await self.connect()
mission = interop_client_bridge.get_mission(mission_id)
mission_dictionary = json.loads(mission)
mission_items = []
mission_items.append(MissionItem(38.1446916666667,
-76.4279944444445,
25,
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
for waypoint in mission_dictionary['waypoints']:
mission_items.append(MissionItem(waypoint['latitude'],
waypoint['longitude'],
waypoint['altitude'],
10,
True,
float('nan'),
float('nan'),
MissionItem.CameraAction.NONE,
float('nan'),
float('nan')))
await self.drone.mission.set_return_to_launch_after_mission(True)
mission_plan = MissionPlan(mission_items)
print("-- Uploading mission")
await self.drone.mission.upload_mission(mission_plan)
async def _mission(drone, lat, lng, wait):
mission_items = []
mission_items.append(
MissionItem(lat, lng, 10, 60, True, 0, 0,
MissionItem.CameraAction.TAKE_PHOTO, 5, float('nan')))
await drone.mission.set_return_to_launch_after_mission(True)
await drone.mission.upload_mission(mission_items)
print(f"-- Uploaded missions")
await drone.action.arm()
print(f"-- Armed")
await drone.mission.start_mission()
print(f"-- Mission has started")
has_landed = False
async def run_scan(self, traj, alt=5, speed=10):
mission_items = []
for p in traj:
mission_items.append(
MissionItem(p[0], p[1], alt, speed, True, float('nan'),
float('nan'), MissionItem.CameraAction.NONE,
float('nan'), float('nan')))
await self._drone.mission.set_return_to_launch_after_mission(True)
print("-- Uploading mission")
await self._drone.mission.upload_mission(mission_items)
print("-- Arming")
await self._drone.action.arm()
print("-- Starting mission")
await self._drone.mission.start_mission()
async def path_mission(goal_loc,
mission_alt=20,
mission_spd=10,
RTL_alt=10,
CAM_pitch=0,
CAM_yaw=0,
VTOL=False):
drone = await connect_sitl()
asyncio.ensure_future(print_mission_progress(drone)) # Parallel task
# termination_task = asyncio.ensure_future(observe_is_in_air(drone)) # keeps script running if drone in air
home_lat, home_lon = await get_lat_lon(drone)
home_loc = home_lat, home_lon
print(f'home location:\n\t>lat:{home_lat}\n\t>lon:{home_lon}')
# await drone.mission.clear_mission() # Clear previous missions stored on drone
path = planned_path(home_loc, goal_loc, GRID_SIZE=200)
mission_items = []
# Takeoff
if not VTOL:
mission_items.append(
MissionItem(home_lat,
home_lon,
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
# Setting Mission waypoints
for waypoint in path:
mission_items.append(
MissionItem(waypoint[0],
waypoint[1],
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
await drone.mission.set_return_to_launch_after_mission(
True) # RTL after last wp
await drone.param.set_float_param("MIS_TAKEOFF_ALT",
mission_alt) # Setting takeoff ALT
await drone.param.set_float_param("RTL_DESCEND_ALT",
RTL_alt) # Setting RTL ALT
await drone.param.set_float_param("RTL_RETURN_ALT",
RTL_alt) # Setting RTL ALT
await drone.mission.upload_mission(mission_items)
await drone.action.arm()
# takeoff and vtol transition with action class [for VTOL]
if VTOL:
await drone.action.takeoff()
await asyncio.sleep(35)
await drone.action.transition_to_fixed_wing()
await asyncio.sleep(30)
await drone.mission.start_mission()
# await termination_task
return drone, home_lat, home_lon
async def search_mission(search_lat,
search_lon,
mission_alt=20,
mission_spd=10,
mission_north=10,
mission_east=10,
mission_south=-10,
RTL_alt=10,
CAM_pitch=0,
CAM_yaw=0):
drone = await connect_sitl()
asyncio.ensure_future(print_mission_progress(drone)) # Parallel task
# termination_task = asyncio.ensure_future(observe_is_in_air(drone)) # keeps script running if drone in air
home_lat, home_lon = await get_lat_lon(drone)
print(f'search location:\n\t>lat:{search_lat}\n\t>lon:{search_lon}')
# await drone.mission.clear_mission() # Clear previous missions stored on drone
mission_items = []
# Takeoff
mission_items.append(
MissionItem(home_lat,
home_lon,
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
# Go to Search Location
mission_items.append(
MissionItem(search_lat,
search_lon,
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
# Setting Mission waypoints
wp_north = get_location_offset_meters(search_lat,
search_lon,
dNorth=mission_north,
dEast=0)
wp_east = get_location_offset_meters(wp_north[0],
wp_north[1],
dNorth=0,
dEast=mission_east)
wp_south = get_location_offset_meters(wp_east[0],
wp_east[1],
dNorth=mission_south,
dEast=0)
# Setting mission waypoints
mission_items.append(
MissionItem(wp_north[0],
wp_north[1],
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
mission_items.append(
MissionItem(wp_east[0],
wp_east[1],
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
mission_items.append(
MissionItem(wp_south[0],
wp_south[1],
mission_alt,
mission_spd,
is_fly_through=True,
gimbal_pitch_deg=CAM_pitch,
gimbal_yaw_deg=CAM_yaw,
camera_action=MissionItem.CameraAction.NONE,
loiter_time_s=float('nan'),
camera_photo_interval_s=float('nan')))
await drone.mission.set_return_to_launch_after_mission(
True) # RTL after last wp
await drone.param.set_float_param("MIS_TAKEOFF_ALT",
mission_alt) # Setting takeoff ALT
await drone.param.set_float_param("RTL_DESCEND_ALT",
RTL_alt) # Setting RTL ALT
await drone.param.set_float_param("RTL_RETURN_ALT",
RTL_alt) # Setting RTL ALT
await drone.mission.upload_mission(mission_items)
await drone.action.arm()
await drone.mission.start_mission()
# await termination_task
return drone, home_lat, home_lon