def create_mission(drone, waypoints):
cmds = drone.commands
cmds.wait_ready()
cmds.clear()
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
waypoints[0]['lat'], waypoints[0]['lon'], 10))
for (i, wp) in enumerate(waypoints):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
wp['lat'], wp['lon'], 10))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
waypoints[-1]['lat'], waypoints[-1]['lon'], 10))
print 'uploading mission...'
cmds.upload()
print 'mission uploaded'
return
def create_command(self, cmd_type, wp, vertical, horizontal, depth):
if (cmd_type == self.command_type.COMMAND_TAKEOFF):
print("In takeoff")
wp = self.get_location_offset_meters(self.home, vertical,
horizontal, depth)
cmd = Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0,
0, wp.lat, wp.lon, wp.alt)
elif (cmd_type == self.command_type.COMMAND_WAYPOINT):
wp = self.get_location_offset_meters(wp, vertical, horizontal,
depth)
cmd = Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0,
0, wp.lat, wp.lon, wp.alt)
elif (cmd_type == self.command_type.COMMAND_LAND):
wp = self.get_location_offset_meters(self.home, vertical,
horizontal, depth)
cmd = Command(0, 0, 0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0,
wp.lat, wp.lon, wp.alt)
else:
print("Invalid command type")
return
self.cmds.add(cmd)
return wp
def quick_scan_adds_mission(vehicle, lla_waypoint_list):
"""
Adds a takeoff command and four waypoint commands to the current mission.
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
for point in lla_waypoint_list:
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
point.lat, point.lon, point.alt))
# Adds dummy end point - this endpoint is the same as the last waypoint and lets us know we have reached destination.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
lla_waypoint_list[-1].lat, lla_waypoint_list[-1].lon,
lla_waypoint_list[-1].alt))
print("Upload new commands to vehicle")
cmds.upload()
def add_mission(lat1,lon1):
"""
Adds a takeoff command and four waypoint commands to the current mission.
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print " Clear any existing commands"
cmds.clear()
print " Define/add new commands."
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
#cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))
#Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, lat1, lon1, 11))
#add dummy waypoint "5" at point 4 (lets us know when have reached destination)
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, vehicle.location.global_relative_frame.lat,vehicle.location.global_relative_frame.lon, 14))
print " Upload new commands to vehicle"
cmds.upload()
def write_mission(mission):
global vehicle
cmd = list()
# ADD TAKEOFF POINT
cmd.append(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0,
mission.home_pos['latitude'], mission.home_pos['longitude'],
0))
# ADD WAYPOINTS
for i in range(len(mission.mission_waypoints)):
cmd.append(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
mission.mission_waypoints[i]['latitude'],
mission.mission_waypoints[i]['longitude'],
mission.mission_waypoints[i]['altitude_msl']))
# ADD LANDING POINT
cmd.append(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0,
mission.home_pos['latitude'], mission.home_pos['longitude'],
1))
return cmd
def add_mission():
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
10))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
point1.lat, point1.lon, 1))
for i in point:
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
i.lat, i.lon, 1))
print(" Upload new commands to vehicle")
cmds.upload()
time.sleep(5)
def quick_scan_adds_mission(vehicle, lla_waypoint_list):
"""
Adds a takeoff command and four waypoint commands to the current mission.
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
# This command is there when vehicle is in AUTO mode, where it takes off through command list
# In guided mode, the actual takeoff function is needed, in which case this command is ignored
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))
# Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
for point in lla_waypoint_list:
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point[0], point[1], 12))
# Adds dummy end point - this endpoint is the same as the last waypoint and lets us know we have reached destination.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0,
0, 0, 0, lla_waypoint_list[-1][0], lla_waypoint_list[-1][1], 12))
# adds dummy end point - this endpoint is the same as the last waypoint and lets us know we have reached destination
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, lla_waypoint_list[-1][0], lla_waypoint_list[-1][1], 12))
print("Upload new commands to vehicle")
cmds.upload()
def adds_mission(landing, altitude):
"""
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
altitude))
#Define the MAV_CMD_NAV_WAYPOINT location and add the commands
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
landing.lat, landing.lon, altitude))
print(" Upload new commands to vehicle")
cmds.upload()
def detailed_search_adds_mission(configs, vehicle):
"""
Only adds a takeoff command for AUTO mission
"""
# Declare shorthands
altitude = configs["altitude"]
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
# Due to a bug presumed to be the fault of DroneKit, the first command is ignored. Thus we have two takeoff commands
if (configs["vehicle_type"] == "VTOL"):
# Separate MAVlink message for a VTOL takeoff. This takes off to altitude and transitions
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0,
0, 0, 0, 0, 0, 0, 0, altitude))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF, 0,
0, 0, 0, 0, 0, 0, 0, altitude))
elif (configs["vehicle_type"] == "Quadcopter"):
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0,
0, 0, 0, 0, 0, 0, altitude))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0,
0, 0, 0, 0, 0, 0, altitude))
# Set the target speed of vehicle
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0,
0, configs["air_speed"], -1, 0, 0, 0, 0))
cmds.upload()
def add_to_follow_mission(next_location, altitude):
"""
Adds the next_location to the waypoint list for a follow-me mission. Preserves the last waypoint as well to help
avoid jittery flight. Should be called every couple of seconds at most
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
# Get the previous waypoint
current_waypoint = cmds.vehicle.commands.next
mission_item = vehicle.commands[current_waypoint-1] # commands are zero indexed
# Convert waypoint into location object
lat = mission_item.x
lon = mission_item.y
alt = mission_item.z
current_waypoint_location = LocationGlobalRelative(lat, lon, alt)
print " Clearing any existing commands"
cmds.clear()
print " Building follow me waypoints"
# Add the previous waypoint.No Takeoff command because it should aready be in the air
cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
0, 0, 0, 0, 0, 0, current_waypoint_location.lat, current_waypoint_location.lon, float(altitude)))
# If the distance between the new location and the previous waypoint exceeds the FOLLOW_THRESHOLD, add to mission
if get_distance_metres(current_waypoint_location, next_location) >= FOLLOW_THRESHOLD:
cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_SPLINE_WAYPOINT,
0, 0, 0, 0, 0, 0, float(next_location.lat), float(next_location.lon), float(altitude)))
return cmds
def detailed_search_adds_mission(vehicle, altitude):
"""
Only adds a takeoff command for AUTO mission
"""
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
# Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
# This command is there when vehicle is in AUTO mode, where it takes off through command list
# In guided mode, the actual takeoff function is needed, in which case this command is ignored
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
altitude))
# Add point directly above vehicle for takeoff
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
vehicle.location.global_frame.lat,
vehicle.location.global_frame.lon, altitude))
cmds.upload()
def adds_mission():
cmds = vehicle_list[index_min].commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
alt_input))
lat_goal = 35.806627
lon_goal = 139.085252
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
35.8051104, 139.0826440, alt_input))
#add dummy waypoint "2" at point 1 (lets us know when have reached destination)
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
lat_goal, lon_goal, alt_input))
print(" Upload new commands to vehicle")
cmds.upload()
def addsParallelTrack(Area, initPoint, alt):
"""
Adds a takeoff command and four waypoint commands to the current mission.
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print " Clear any existing commands"
cmds.clear()
print " Define/add new commands."
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Calculate track properties
trackLength = 2 * math.sqrt(dFSA / math.pi)
legConst = 5 #Arbitrary ratio of leg length to track length
legLength = legConst * trackLength #Leg length function of dFSA radius
numLegs = Area / (trackLength * legLength)
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
alt))
#Go to initial point as specified by user
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
initPoint.lat, initPoint.lon, alt))
#Define waypoint pattern - point(lat, long, alt)
i = 1
waypoint = initPoint
while i <= numLegs:
# Strafe
waypoint = get_location_metres(waypoint, 0, (legLength * (-1)**i))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
waypoint.lat, waypoint.lon, alt))
# Advance
waypoint = get_location_metres(waypoint, trackLength, 0)
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
waypoint.lat, waypoint.lon, alt))
i += 1
# Return to Launch
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
initPoint.lat, initPoint.lon, alt))
print " Upload new commands to vehicle"
cmds.upload()
def build_loop_mission(vehicle, loop_center, loop_radius, altitude):
"""
Adds a takeoff command and 12 waypoint commands to the current mission.
The waypoints are positioned to form a dodecagon with vertices at loop_radius around the specified
LocationGlobal (loop_center).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
Modified from Dronekit-python
"""
cmds = vehicle.commands
print ( " Clearing any existing commands")
cmds.clear()
print ( " Building loop waypoints.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
# Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
0, 0, 0, 0, 0, 0, 0, 0, 10))
# Define the twelve MAV_CMD_NAV_WAYPOINT locations and add the commands
for n in range(0, 11, 1):
d_north = math.sin(math.radians(n*30))*loop_radius
d_east = math.cos(math.radians(n*30))*loop_radius
point = get_location_metres(loop_center, d_north, d_east)
cmds.add(Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, 0, 0, 0, 0, 0, point.lat, point.lon, altitude))
print ( " Upload new commands to vehicle")
cmds.upload()
def addsParallelTrack(Area, initPoint, alt):
"""
Adds mission to perform Parallel Track across specified area.
"""
dFSA = 30 # Functional Search Area (m)
cmds = vehicle.commands
print ( " Clear any existing commands")
cmds.clear()
print ( " Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Calculate track properties
trackLength = 2 * math.sqrt(dFSA/math.pi)
legConst = 5 #Arbitrary ratio of leg length to track length
legLength = legConst * trackLength #Leg length function of dFSA radius
numLegs = Area / (trackLength * legLength)
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, alt))
#Go to initial point as specified by user
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, initPoint.lat, initPoint.lon, alt))
#Define waypoint pattern - point(lat, long, alt)
i = 1;
waypoint = initPoint
while i <= numLegs :
# Strafe
waypoint = get_location_metres(waypoint, 0, (legLength*(-1)**i))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoint.lat, waypoint.lon, alt))
# Advance
waypoint = get_location_metres(waypoint, trackLength, 0)
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoint.lat, waypoint.lon, alt))
i += 1
# Return to Launch
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, initPoint.lat, initPoint.lon, alt))
print ( " Upload new commands to vehicle")
cmds.upload()
def adds_square_mission(vehicle, aLocation, aSize):
"""
Adds a takeoff command and four waypoint commands to the current mission.
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
"""
cmds = vehicle.commands
print " Clear any existing commands"
cmds.clear()
print " Define/add new commands."
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
point1 = get_location_metres(aLocation, aSize, -aSize)
point2 = get_location_metres(aLocation, aSize, aSize)
point3 = get_location_metres(aLocation, -aSize, aSize)
point4 = get_location_metres(aLocation, -aSize, -aSize)
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 10))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point1.lat, point1.lon, 10))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point2.lat, point2.lon, 10))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, point3.lat, point3.lon, 10))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 5, 0, 0, 0, point4.lat, point4.lon, 10))
print " Upload new commands to vehicle"
cmds.upload()
def add_mission():
global waypoint_list, no_of_waypoint
# declaring object of vehicle.commands and importing data simultaneously
cmds = vehicle.commands
# clearing data
print(" Clear any existing commands")
cmds.clear()
# Using for loop to add the command to cmds list
print(" Define/add new commands.")
for i in range(0, no_of_waypoint):
# making the point variable as an instance of LocationGlobal and format for lat and lon
point = LocationGlobal(waypoint_list[i][0], waypoint_list[i][1], 0)
# Adding the GPS points after extraction over mavlink command
# last 3 parameters are for lat, lon, alt
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
point.lat, point.lon, waypoint_list[i][2]))
# Adding an RTL - Return to launch command at the end of the mission
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0,
0, 0, 10))
# uploading new commands to the drone
print(" Upload new commands to vehicle")
cmds.upload()
def test_get_waypoint(self, commands_mock):
commands_mock.return_value.configure_mock(__getitem__=Mock(
side_effect=IndexError))
self.assertIsNone(self.vehicle.get_waypoint(waypoint=0))
cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM, 0, 0, 0, 0, 0,
0, 0, 0, 0)
commands_mock.return_value.configure_mock(__getitem__=Mock(
return_value=cmd))
self.assertIsNone(self.vehicle.get_waypoint(waypoint=1))
cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
3.4, 2.3, 1.2)
commands_mock.return_value.configure_mock(__getitem__=Mock(
return_value=cmd))
self.assertEqual(self.vehicle.get_waypoint(),
LocationLocal(3.4, 2.3, -1.2))
with patch.object(self.vehicle, "_geometry", new=Geometry_Spherical()):
self.assertEqual(self.vehicle.get_waypoint(),
LocationGlobalRelative(3.4, 2.3, 1.2))
def addsParallelTrack(aLocation, dLat, dLong ,numLegs, alt):
"""
Adds mission to parallel track search search across specified area.
"""
cmds = vehicle.commands
print " Clear any existing commands"
cmds.clear()
print " Define/add new commands."
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, alt))
#Define waypoint pattern - point(lat, long, alt)
i = 1;
waypoint = aLocation
while i <= numLegs :
# Strafe
waypoint = get_location_metres(waypoint, 0, (dLat*(-1)**i))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoint.lat, waypoint.lon, alt))
# Advance
waypoint = get_location_metres(waypoint, dLong, 0)
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, waypoint.lat, waypoint.lon, alt))
i += 1
# Return to Launch
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, aLocation.lat, aLocation.lon, alt))
print " Upload new commands to vehicle"
cmds.upload()
def add_mission(aLocations):
'''
The waypoints are positioned to form a square of side length 2*aSize around the specified LocationGlobal (aLocation).
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
'''
cmds = vehicle.commands
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
# Add new commands. The meaning/order of the parameters is documented in the Command class.
# Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 10))
for location in aLocations:
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, location[1], location[0], 11))
location = aLocations[0]
# Dummy waypoint
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, location[1], location[0], 11))
print(" Upload new commands to vehicle")
cmds.upload()
def toTarget(vehicle, n, e):
# load commands
cmds = vehicle.commands
cmds.clear()
home = vehicle.location.global_relative_frame
# takeoff to 3 meters
wp = get_location_offset_meters(home, 0, 0, 2)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,\
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0,\
wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# move to target
wp = get_location_offset_meters(wp, -n, -e, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,\
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0,\
wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# land
wp = get_location_offset_meters(wp, 0, 0, 0)
cmd = Command(0,0,0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,\
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 1, 0, 0, 0, 0,\
wp.lat, wp.lon, wp.alt)
cmds.add(cmd)
# upload mission
print('Uploading mission')
cmds.upload()
time.sleep(2)
def SCGI_mission(ArrayList, Altura_wps):
cmds = vehicle.commands
print " Clear any existing commands"
cmds.clear()
print " Define/add new commands."
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
Altura_wps))
for Wps in ArrayList:
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
pointsW = Wps.split(",")
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
float(pointsW[0]), float(pointsW[1]), Altura_wps))
#add dummy waypoint "n" at point n-1 (lets us know when have reached destination)
LastData = len(ArrayList) - 1
lastpoint = ArrayList[LastData]
sep = lastpoint.split(",")
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0,
vehicle.home_location.lat, vehicle.home_location.lon, 0))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
vehicle.home_location.lat, vehicle.home_location.lon,
Altura_wps))
print " Upload new commands to vehicle"
cmds.upload()
def build_hover_mission(vehicle, hover_location, altitude):
"""
Adds a takeoff command and waypoint to the current mission. Used for Hover mode or to begin a Follow-Me mode flight
The function assumes vehicle.commands matches the vehicle mission state
(you must have called download at least once in the session and after clearing the mission)
Modified from Dronekit-python
"""
cmds = vehicle.commands
print " Clearing any existing commands"
cmds.clear()
print " Building hover waypoint"
# Add new commands. The meaning/order of the parameters is documented in the Command class.
# Add MAV_CMD_NAV_TAKEOFF command. This is ignored if the vehicle is already in the air.
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0,
10))
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
hover_location.lat, hover_location.lon, altitude))
print " Upload new commands to vehicle"
cmds.upload()
def __uploadCommandsIntoSoloMemoryFastLanding__(self):
cmds = self.vehicle.commands
#index for association between picture and location
index = 0
#open the file for the picture-location association
pictures_file = open("association picture-location Solo " + self.name + ".txt", "a")
pictures_file.write("Survey: " + str(time.strftime("%c")) + "\n")
changeSpeedCommand = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_CHANGE_SPEED, 0, 0, 0, 5, 0, 1, 0, 0, 0)
cmds.add(changeSpeedCommand)
bearingCommand = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_CONDITION_YAW, 0, 0, self.bearing, 10, 1, 0, 0, 0, 0)
cmds.add(bearingCommand)
#adding waypoint and takeAPicture commands
for location in self.listOfLocationsToReach:
locationCommand = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, location.lat, location.lon, location.alt)
pictureCommand = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL, 0, 0, 1, 0, 0, 0, 1, 0, 0)
pictures_file.write("Index " + str(index) + " ---> " + "lat: " + str(location.lat) + ", lon: " + str(location.lon) + ", alt: " + str(location.alt) + "\n")
index = index + 1
cmds.add(locationCommand)
cmds.add(pictureCommand)
pictures_file.write("\n###########################################\n\n")
pictures_file.close()
#adding command for returning home when the mission will be ended
RTLCommand = Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0, 0, 0, 0, 0, 0, 0)
cmds.add(RTLCommand)
#taking off command
self.__armAndTakeOff__()
#uploading commands to UAV
cmds.upload()
self.vehicle.commands.next = 0 #reset mission set to first(0) waypoint
def añadirMision(waypoints, altura):
global wp
wp = waypoints
cmds = vehicle.commands
my_location_alt = vehicle.location.global_frame
my_location_alt.alt = 0.0
vehicle.home_location = my_location_alt
# vehicle.location.global_frame = my_location_alt
print(" New Home Location (from attribute - altitude should be 222): %s" % vehicle.home_location)
print(" Clear any existing commands")
cmds.clear()
print(" Define/add new commands.")
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, altura))
for Wps in waypoints:
# Add new commands. The meaning/order of the parameters is documented in the Command class.
#Define the four MAV_CMD_NAV_WAYPOINT locations and add the commands
pointsW=Wps.split(",")
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 5, 0, 0, 0, float(pointsW[0]), float(pointsW[1]), altura))
# cmds.add(Command())
print(cmds)
#add dummy waypoint "n" at point n-1 (lets us know when have reached destination)
LastData=len(waypoints)-1
lastpoint=waypoints[LastData]
sep=lastpoint.split(",")
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0, vehicle.home_location.lat, vehicle.home_location.lon, altura))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_LAND, 0, 0, 0, 0, 0, 0, vehicle.home_location.lat,vehicle.home_location.lon, 0))
print(" Upload new commands to vehicle")
cmds.upload()
def second_tour(lat, lon):
cmds = vehicle.commands
cmds.clear()
vehicle.flush()
#cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 5)) # Delete in real fligth
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
36.9778569, 37.3033670, 5)) # 4
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
36.9779463, 37.303367, 5)) # 5
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
36.9779731, 37.3033456, 5)) # 60m başlangıç
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, lat,
lon, 6)) # Su bırakma alanı
cmds.add(
Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, lat,
lon, 6)) # Dummy Su bırakma alanı
cmds.upload()
def createLoiterTimeCmd(time_to_loiter, loiter_radius, altitude, location):
if time_to_loiter >= 1200:
newcmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,0,0, 0, 0, loiter_radius, 0,
location[0], location[1], altitude)
else:
newcmd = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,0,0,time_to_loiter,0,loiter_radius,0,location[0], location[1], altitude)
return newcmd
def createLoiterCmd(number_of_loiters, loiter_radius, altitude, location):
# loiter for an unlimited amount of time until next command
if number_of_loiters == 999:
newcmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,0,0, 0, 0, loiter_radius, 0,
location[0], location[1], altitude)
else:
newcmd = Command(0,0,0,mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,0,0, number_of_loiters, 0, loiter_radius, 0, location[0], location[1], altitude)
return newcmd
def setPath(self, request, context):
"""Uploads a series of positions to the drone in order to create a mission."""
cmds = self.vehicle.commands
coordFrame, alt = None, None
# The idea behind stripping off the first position is to determine what reference frame to
# to use. Future proto changes will removed the coordinate frame boolean flag from the
# request making the code unnecessary. For now, this is the way it is.
firstPosition = nth(request, 0)
lat = firstPosition.lat
lon = firstPosition.lon
useRelativeAltitude = firstPosition.useRelativeAltitude
if useRelativeAltitude:
alt = firstPosition.relativeAltitude
coordFrame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
else:
alt = firstPosition.gpsAltitude
coordFrame = mavutil.mavlink.MAV_FRAME_GLOBAL
print(('First position at ({0},{1}) -> {2}'.format(lat, lon, alt)))
# Make sure the drone is not in AUTO mode.
self.vehicle.mode = VehicleMode("LOITER")
self.clear_mission(cmds, coordFrame)
# Add first position
cmds.add(
Command(0, 0, 0, coordFrame, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, 0, 0, 0, 0, 0, lat, lon, alt))
# Add the remaining positions
for position in request:
lat = position.lat
lon = position.lon
if useRelativeAltitude:
alt = position.relativeAltitude
else:
alt = position.gpsAltitude
print(('Point at ({0},{1}) -> {2}'.format(lat, lon, alt)))
cmds.add(
Command(0, 0, 0, coordFrame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, 0, 0, 0,
lat, lon, alt))
print("Uploading new commands to drone")
cmds.upload()
print("Starting mission")
# Reset mission set to first (0) waypoint
self.vehicle.commands.next = 0
self.vehicle.mode = VehicleMode("AUTO")
self.print_mission()
return droneconnect_pb2.Null()
def first_tour():
cmds = vehicle.commands
cmds.clear()
vehicle.flush()
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 0, 0, 0, 0, 0, 0, 5))
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6946396 ,35.4597401 , 5))#1 Su alma alanı
cmds.add(Command( 0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 1, 0, 0, 0, 38.6946396 ,35.4597401 , 5))#1 Su alma alanı
print(" Upload new commands to vehicle")
cmds.upload()
