Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.
if type == 'MISSION_ITEM':
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if type == 'CMD':
if options.mission is None:
m = mavutil.mavlink.MAVLink_mission_item_message(0,
0,
m.CNum,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
m.CId,
0, 1,
m.Prm1, m.Prm2, m.Prm3, m.Prm4,
m.Lat, m.Lng, m.Alt)
try:
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
except Exception:
pass
continue
if not mlog.check_condition(options.condition):
continue
if options.mode is not None and mlog.flightmode.lower() != options.mode.lower():
continue
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
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)
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, 11))
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, 12))
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, 13))
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, point4.lat, point4.lon, 14))
#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, point4.lat, point4.lon, 14))
print(" Upload new commands to vehicle")
cmds.upload()
dp += self.unitVector.y * vectorToTarget.y
dp += self.unitVector.z * vectorToTarget.z
self.actualDistance = location_helpers.getDistanceFromPoints3d(self.initialLocation, self.vehicle.location.global_relative_frame)
if (dp < 0):
self.actualDistance = -self.actualDistance
# We can now compare the actual vs vector distance
self.distError = self.actualDistance - self.distance
# formulate mavlink message for pos-vel controller
posVelMsg = self.vehicle.message_factory.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0, 1, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
0b0000110111000000, # type_mask - enable pos/vel
int(loc.lat * 10000000), # latitude (degrees*1.0e7)
int(loc.lon * 10000000), # longitude (degrees*1.0e7)
loc.alt, # altitude (meters)
velVector.x, velVector.y, velVector.z, # North, East, Down velocity (m/s)
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send pos-vel command to vehicle
self.vehicle.send_mavlink(posVelMsg)
def set_yaw(self, heading):
# create the CONDITION_YAW command
msg = self.vehicle.message_factory.mission_item_encode(0, 0, # target system, target component
0, # sequence
mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
mavlink.MAV_CMD_CONDITION_YAW, # command
2, # current - set to 2 to make it a guided command
0, # auto continue
heading, 0, 0, 0, 0, 0, 0) # param 1 ~ 7
# send command to vehicle
self.vehicle.send_mavlink(msg)
self.vehicle.flush()
newRoi.alt = tmp
self.addLocation(newRoi)
self.updateROIAlt(channels[RAW_PADDLE])
# nothing to do if no user interaction
if not self.needsUpdate:
return
# clear update flag
self.needsUpdate = False
# Tell Gimbal ROI Location
msg = self.vehicle.message_factory.command_int_encode(
0, 1, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, #frame
mavutil.mavlink.MAV_CMD_DO_SET_ROI, #command
0, #current
0, #autocontinue
0, 0, 0, 0, #params 1-4
self.roi.lat*1.E7,
self.roi.lon*1.E7,
self.roi.alt)
self.vehicle.send_mavlink(msg)
# learn any changes to controller alt offset to apply it to other controllers when instantiated (to avoid jerks)
self.followControllerAltOffset = pos.alt - self.filteredROI.alt
# mavlink expects 10^7 integer for accuracy
latInt = int(pos.lat * 10000000)
lonInt = int(pos.lon * 10000000)
# Convert from NEU to NED to send to autopilot
vel.z *= -1
# create the SET_POSITION_TARGET_GLOBAL_INT command
msg = self.vehicle.message_factory.set_position_target_global_int_encode(
0, # time_boot_ms (not used)
0, 1, # target system, target component
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
0b0000110111000000, # Pos Vel type_mask
latInt, lonInt, pos.alt, # x, y, z positions
vel.x, vel.y, vel.z, # x, y, z velocity in m/s
0, 0, 0, # x, y, z acceleration (not used)
0, 0) # yaw, yaw_rate (not used)
# send command to vehicle
self.vehicle.send_mavlink(msg)
''' Pointing Control'''
# If followState mandates that the user controls the pointing
if self.followState == FOLLOW_FREELOOK:
self.manualPitch(channels[THROTTLE])
self.manualYaw(channels)
def condition_yaw(heading):
msg = vehicle.message_factory.mission_item_encode(0, 0, # target system, target component
0, # sequence
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame
mavutil.mavlink.MAV_CMD_CONDITION_YAW, # command
2, # current - set to 2 to make it a guided command
0, # auto continue
heading, # param 1, yaw in degrees
0, # param 2, yaw speed deg/s
1, # param 3, direction -1 ccw, 1 cw
0, # param 4, relative offset 1, absolute angle 0
0, 0, 0) # param 5 ~ 7 not used
# send command to vehicle
vehicle.send_mavlink(msg)
vehicle.flush()
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
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)
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, 11))
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, 12))
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, 13))
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, point4.lat, point4.lon, 14))
#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, point4.lat, point4.lon, 14))
print(" Upload new commands to vehicle")
cmds.upload()
cmds.wait_ready()
cmds = vehicle.commands
cmds.clear()
for pt in messages:
#print "Point: %d %d" % (pt.lat, pt.lon,)
lat = pt.lat
lon = pt.lon
# To prevent accidents we don't trust the altitude in the original flight, instead
# we just put in a conservative cruising altitude.
altitude = 30.0
cmd = Command( 0,
0,
0,
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, 0, 0, 0, 0, 0,
lat/1.0e7, lon/1.0e7, altitude)
cmds.add(cmd)
#Upload clear message and command messages to vehicle.
print("Uploading %d waypoints to vehicle..." % len(messages))
cmds.upload()
print("Arm and Takeoff")
arm_and_takeoff(30)
print("Starting mission")
# Reset mission set to first (0) waypoint