def fix_alt(filename, agl):
'''fix AGL on mission'''
wp = mavwp.MAVWPLoader()
wp.load(filename)
home = wp.wp(0)
if opts.home:
a = opts.home.split(",")
home.x = float(a[0])
home.y = float(a[1])
home_agl = get_ground_alt(home.x, home.y)
print("Home AGL %.1f" % home_agl)
for i in range(1, wp.count()):
w = wp.wp(i)
if (w.x == 0 and w.y == 0) or w.command not in [
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
mavutil.mavlink.MAV_CMD_NAV_LOITER_UNLIM,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS,
mavutil.mavlink.MAV_CMD_NAV_LOITER_TIME,
mavutil.mavlink.MAV_CMD_NAV_LAND,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF
]:
continue
ground = get_ground_alt(w.x, w.y)
w.z = (ground - home_agl) + float(agl)
print("ground Elevation %.1f z=%.1f" % (ground, w.z))
wp.save(opts.output)
print("Saved %s" % opts.output)
return wp
def set_waypoints(connect_address:int, waypoints:list)->bool:
'''waypoints should be given in this form:
[(lat0,lon10,alt0), (lat1,lon1,alt1), ...]'''
try:
mavlink = mavutil.mavlink_connection(SERVER_IP+':'+str(connect_address))
mavlink.wait_heartbeat(timeout=6)
wp = mavwp.MAVWPLoader()
seq = 1
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
radius = 1
waypoints = [(0,0,0)]+waypoints
for i, waypoint in enumerate(waypoints):
wp.add(mavutil.mavlink.MAVLink_mission_item_message(mavlink.target_system,
mavlink.target_component,
seq,
frame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT,
0, 0, 0, radius, 0, 0,
waypoint[0],waypoint[1],waypoint[2]))
seq += 1
mavlink.waypoint_clear_all_send()
ack_msg = get_ack_msg(connect_address, mavlink, ['WAYPOINT_REQUEST', 'MISSION_ACK', 'MISSION_REQUEST'], should_send=True, command_name='WAYPOINT_CLEAR_ALL')
mavlink.waypoint_count_send(wp.count())
for i in range(wp.count()):
ack_msg = get_ack_msg(connect_address, mavlink, ['WAYPOINT_REQUEST', 'MISSION_ACK', 'MISSION_REQUEST'], should_send=True)
mavlink.mav.send(wp.wp(ack_msg['seq']))
ack_msg = get_ack_msg(connect_address, mavlink, ['WAYPOINT_REQUEST', 'MISSION_ACK', 'MISSION_REQUEST'])
return ack_msg
except Exception as e:
print(e)
return {'ERROR': 'Set waypoint failed!'+str(e), 'droneid':connect_address}
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
m = mlog.recv_match(type=['MISSION_ITEM','CMD','WAYPOINT'])
if m is None:
break
if m.get_type() == 'CMD':
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)
if m.current >= 2:
continue
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
wp.save(args.output)
print("Saved %u waypoints to %s" % (wp.count(), args.output))
def test_add_remove(self):
"""Test we can add/remove waypoints to/from mavwp"""
loader = mavwp.MAVWPLoader()
waypoints = self.make_wps()
# make sure basic addition works
for i in range(len(waypoints)):
self.assertEqual(loader.count(), i)
loader.add(waypoints[i])
self.assertEqual(loader.count(), i + 1)
self.assertEqual(loader.wp(0).seq, 0)
self.assertEqual(loader.wp(1).seq, 1)
self.assertEqual(loader.wp(2).seq, 2)
self.assertEqual(loader.wp(0).z, 0)
self.assertEqual(loader.wp(1).z, 1)
self.assertEqual(loader.wp(2).z, 2)
# remove a middle one, make sure things get renumbered
loader.remove(waypoints[0])
self.assertEqual(loader.wp(0).seq, 0)
self.assertEqual(loader.wp(1).seq, 1)
self.assertEqual(loader.wp(2).seq, 2)
self.assertEqual(loader.wp(0).z, 1)
self.assertEqual(loader.wp(1).z, 2)
self.assertEqual(loader.wp(2).z, 3)
loader.clear()
self.assertEqual(loader.count(), 0)
def startMission(self):
print("Starting mission")
self.set_mode_loiter()
self.arm()
self.master.mav.command_long_send(
self.master.target_system,
self.master.target_component, # target_component
mavutil.mavlink.MAV_CMD_MISSION_START, # command
0,
1, # first WP
mavwp.MAVWPLoader().count(), # last WP
0,
0,
0,
0,
0)
msg = self.master.recv_match(
type=['COMMAND_ACK'],
condition='COMMAND_ACK.command==' +
str(mavutil.mavlink.MAV_CMD_MISSION_START),
blocking=True)
if (msg.result != 0):
raise Exception("Unable to start mission")
print("Mission started")
def loadFlightPlanFile(ac, consumer_message, q, master, mlog):
try:
logger = logging.getLogger()
wpload = mavwp.MAVWPLoader(master.target_system,
master.target_component)
dir = os.path.dirname(os.path.realpath(__file__))
num = wpload.load(dir + '/../Examples/FlightPlans/' +
consumer_message[1])
msg_list = []
vel = 0
for i in range(0, num):
msg_item = wpload.wp(i)
if msg_item.command == 16 or msg_item.command == 22:
msg_part = '{ "SEQ":' + str(msg_item.seq) + \
', "LAT":' + str(msg_item.x) + \
', "LNG":' + str(msg_item.y) + \
', "ALT":' + str(msg_item.z) + '}'
msg_list.append(msg_part)
elif msg_item.command == 178:
vel = msg_item.param2
msg = '{"AIRCRAFT":' + ac + ',' + '"TYPE":"WP", "LIST":['
msg = msg + (
', ').join(msg_list) + '], "VEL":' + str(vel) + ', "FILE":"true"}'
q.put(msg)
q.put('{"AIRCRAFT":' + ac +
', "name":"LOAD" , "INFO":"SUCCESS", "MSG":"LOAD WP FILE"}')
except Exception as e:
q.put('{"AIRCRAFT":' + ac +
', "name":"LOAD" , "INFO":"FAIL", "MSG":"' + str(e) + '"}')
def set_mission_from_positions(
self,
position_targets,
altitude=20,
radius=5,
frame=mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT):
waypoint_loader = mavwp.MAVWPLoader()
for position_ind, position in enumerate(position_targets):
print("The current target position is %s" % position_ind)
waypoint = mavlink.MAVLink_mission_item_message(
self.mav_connection.target_system,
self.mav_connection.target_component, position_ind, frame,
mavlink.MAV_CMD_NAV_WAYPOINT, 0, 0, 0, radius, 0, 0,
position[0], position[1], altitude)
waypoint_loader.add(waypoint)
self.clear_mission()
self.mav_link.mission_count_send(self.mav_connection.target_system,
self.mav_connection.target_component,
len(position_targets))
print("I am done telling how many targets there are {0}".format(
len(position_targets)))
self.handle_mission_transmission(waypoint_loader)
def __init__(self, mavfile):
self.mavfile = mavfile
self.loader = mavwp.MAVWPLoader(target_system=1, target_component=190)
self.count = 0
self.send_time = 0
self.recv_time = 0
self._transition_idle()
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
m = mlog.recv_match(
type=['MISSION_ITEM', 'CMD', 'WAYPOINT', 'MISSION_ITEM_INT'])
if m is None:
break
if m.get_type() == 'CMD':
try:
frame = m.Frame
except AttributeError:
print("Warning: assuming frame is GLOBAL_RELATIVE_ALT")
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
m = mavutil.mavlink.MAVLink_mission_item_message(
0, 0, m.CNum, frame, m.CId, 0, 1, m.Prm1, m.Prm2, m.Prm3,
m.Prm4, m.Lat, m.Lng, m.Alt)
if m.get_type() == 'MISSION_ITEM_INT':
m = mavutil.mavlink.MAVLink_mission_item_message(
m.target_system, m.target_component, m.seq, m.frame, m.command,
m.current, m.autocontinue, m.param1, m.param2, m.param3,
m.param4, m.x * 1.0e-7, m.y * 1.0e-7, m.z)
if m.current >= 2:
continue
while m.seq > wp.count():
print("Adding dummy WP %u" % wp.count())
wp.set(m, wp.count())
wp.set(m, m.seq)
wp.save(args.output)
print("Saved %u waypoints to %s" % (wp.count(), args.output))
def __init__(self, mpstate):
super(WPModule, self).__init__(mpstate,
"wp",
"waypoint handling",
public=True)
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.undo_wp = None
self.undo_type = None
self.undo_wp_idx = -1
self.add_command('wp', self.cmd_wp, 'waypoint management', [
"<list|clear|move|remove|loop|set|undo|movemulti|changealt|param|status>",
"<load|update|save|show> (FILENAME)"
])
if self.continue_mode and self.logdir != None:
waytxt = os.path.join(mpstate.status.logdir, 'way.txt')
if os.path.exists(waytxt):
self.wploader.load(waytxt)
print("Loaded waypoints from %s" % waytxt)
self.menu_added_console = False
self.menu_added_map = False
if mp_util.has_wxpython:
self.menu = MPMenuSubMenu(
'Mission',
items=[
MPMenuItem('Editor', 'Editor', '# wp editor'),
MPMenuItem('Clear', 'Clear', '# wp clear'),
MPMenuItem('List', 'List', '# wp list'),
MPMenuItem('Load',
'Load',
'# wp load ',
handler=MPMenuCallFileDialog(
flags=('open', ),
title='Mission Load',
wildcard='*.txt')),
MPMenuItem('Save',
'Save',
'# wp save ',
handler=MPMenuCallFileDialog(
flags=('save', 'overwrite_prompt'),
title='Mission Save',
wildcard='*.txt')),
MPMenuItem('Draw',
'Draw',
'# wp draw ',
handler=MPMenuCallTextDialog(
title='Mission Altitude (m)', default=100)),
MPMenuItem('Undo', 'Undo', '# wp undo'),
MPMenuItem('Loop', 'Loop', '# wp loop')
])
def __init__(self, mpstate):
"""Initialise module"""
super(AUVModule, self).__init__(mpstate, "auto",
"Autonomous navigation module")
'''Navigational information'''
self.waypoints = deque()
self.next_wp = [0, 0] # lat,lng
self.wp_perimeter = (0, 0, 0, 0)
self.reached_wp = False
self.offset_from_intended_heading = 0
self.orienting = False
self.loops = 0
self.xy = {'x': 0, 'y': 0} # x,y
self.y = True
self.dense = False
self.start_time = 0
self.home = {'lat': 0, 'lng': 0}
'''Attitude'''
self.lat = 0
self.lon = 0
self.alt = 0
self.relative_alt = 0
self.vx = 0
self.vy = 1
self.vz = 1
self.hdg = 0
'''Battery information'''
self.battery_level = -1
self.voltage_level = -1
self.current_battery = -1
self.last_batt = time()
self.ujoules = 0
self.write_to_battery = []
'''Pressure and Depth Sensors'''
self.temp_sensor = [0] * 3
self.depth_sensor = [0] * 3
'''Infinite sized queue for motor commands'''
self.command_queue = deque()
self.end_time = 0
self.motor_run_time = 0
self.last_orient_check = 0
'''Sampling'''
self.last_sample = time()
self.sensor_reader = SerialReader.SerialReader()
''' Commands for operating the module from the MAVProxy CLI'''
self.add_command('auto', self.cmd_auto,
"Autonomous sampling traversal",
['home', 'test', 'mission', 'setfence'])
'''low-level pymavlink functionality'''
self.wp_loader = mavwp.MAVWPLoader(self.target_system,
self.target_component)
'''Failsafe checks'''
self.time_at_last_heartbeat = time()
self.servo_output_raw = []
self.rc_channels_raw = []
self.rc_regexp = compile('chan[1-6]_raw')
self.servo_regexp = compile('servo[1-6]_raw')
self.mav_state_critical = False
self.mav_state_emergency = False
def load_mission_from_file(mavproxy, mav, filename):
'''load a mission from a file'''
global num_wp
wploader = mavwp.MAVWPLoader()
wploader.load(filename)
num_wp = wploader.count()
print("loaded mission with %u waypoints" % num_wp)
return True
def __init__(self):
self.wp_op = None
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
def LaunchArducopter(scenario):
wploader = mavwp.MAVWPLoader()
wploader.load(os.path.join(icarous_home, scenario["waypoint_file"]))
wp0 = wploader.wp(0)
start_point = ','.join(str(x) for x in [wp0.x, wp0.y, wp0.z, 0])
ap = subprocess.Popen(["sim_vehicle.py", "-v", "ArduCopter",
"-l", str(start_point)],
stdout=subprocess.DEVNULL)
time.sleep(60)
def test_is_location_command(self):
loader = mavwp.MAVWPLoader()
self.assertFalse(
loader.is_location_command(
mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH))
self.assertTrue(
loader.is_location_command(mavutil.mavlink.MAV_CMD_NAV_WAYPOINT))
self.assertTrue(
loader.is_location_command(
mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS))
def test_save(self):
'''test load/save in default format'''
loader = mavwp.MAVWPLoader()
waypoints = self.make_wps()
for wp in waypoints:
loader.add(wp)
outfile = "test_wp-wp.txt"
loader.save(outfile)
loader2 = mavwp.MAVWPLoader()
loader2.load(outfile)
outfile2 = "test_wp-wp2.txt"
loader.save(outfile2)
self.assertEqual(loader.count(), loader2.count())
for i in range(loader.count()):
self.assertEqual(loader.wp(i).z, loader2.wp(i).z)
def __init__(self, master, target_system, target_component):
self.master = master
self.fenceList = []
self.sentFenceList = []
self.fenceToSend = 0
self.have_list = False
self.wploader = mavwp.MAVWPLoader()
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.target_system = target_system
self.target_component = target_component
self.traffic_list = []
self.start_lat = 0
self.start_lon = 0
def load_mission_from_file(self, filename):
"""Load a mission from a file to flight controller."""
self.mavproxy.send('wp load %s\n' % filename)
self.mavproxy.expect('Flight plan received')
self.mavproxy.send('wp list\n')
self.mavproxy.expect('Requesting [0-9]+ waypoints')
# update num_wp
wploader = mavwp.MAVWPLoader()
wploader.load(filename)
num_wp = wploader.count()
return num_wp
def main():
mission_path = None
if len(sys.argv) > 1:
mission_path = sys.argv[1]
frame = '+'
# If we were given a mission, use its first waypoint as home.
if mission_path:
wploader = mavwp.MAVWPLoader()
wploader.load(mission_path)
wp = wploader.wp(0)
home = mavutil.location(wp.x, wp.y, wp.z, 0)
run_mission(mission_path, frame, home)
def load_mission_from_file(mavproxy, mav, filename):
'''Load a mission from a file to flight controller'''
global num_wp
mavproxy.send('wp load %s\n' % filename)
mavproxy.expect('flight plan received')
mavproxy.send('wp list\n')
mavproxy.expect('Requesting [0-9]+ waypoints')
# update num_wp
wploader = mavwp.MAVWPLoader()
wploader.load(filename)
num_wp = wploader.count()
return True
def networkingProcess(server, port, manager, debug):
# Connect to server
print("Connecting to ", server, ":", port, sep="")
# Connect to MAVProxy ground station
master = mavutil.mavlink_connection(server + ":" + str(port))
# Wait for a heartbeat so we know the target system IDs
print("Waiting for heartbeat")
master.wait_heartbeat()
# Wait till armed
print("Waiting for motors to be armed")
master.motors_armed_wait()
# Set that we want to receive data
print("Requesting data")
rate = 25 # Hz
master.mav.request_data_stream_send(
master.target_system,
master.target_component,
mavutil.mavlink.MAV_DATA_STREAM_ALL, rate, 1)
# Manage waypoints, so we can get the home point
wp = mavwp.MAVWPLoader()
#
# Set glider settings
#
# "Glider Pilots: Set this parameter to 2.0 (The glider will adjust its
# pitch angle to maintain airspeed, ignoring changes in height)."
#
# See: http://ardupilot.org/plane/docs/tecs-total-energy-control-system-for-speed-height-tuning-guide.html
#
params = mavparm.MAVParmDict()
params.mavset(master, b'TECS_SPDWEIGHT', 2.0)
params.mavset(master, b'WP_LOITER_RAD', turnRadius)
# Request the home point, so we can roughly know how high above our
# starting point we are
master.waypoint_request_send(0)
# Start send/recieve threads
receive = NetworkingThreadReceive(master, manager, wp, debug)
send = NetworkingThreadSend(master, manager, wp, debug)
receive.start()
send.start()
receive.join()
send.join()
print("Exiting networkingProcess")
def mavmission(logfile):
'''extract mavlink mission'''
mlog = mavutil.mavlink_connection(filename)
wp = mavwp.MAVWPLoader()
while True:
if mlog.mavlink10():
m = mlog.recv_match(type='MISSION_ITEM')
else:
m = mlog.recv_match(type='WAYPOINT')
if m is None:
break
wp.set(m, m.seq)
wp.save(opts.output)
print("Saved %u waypoints to %s" % (wp.count(), opts.output))
def writeWayPoints(waypoints):
global master
master.wait_heartbeat()
print("HEARTBEAT OK\n")
# Make Waypoints
wp = mavwp.MAVWPLoader()
seq = 0
for waypoint in enumerate(waypoints):
frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
seq = waypoint[0]
lat, lon = waypoint[1]
altitude = 15 # 15 meter
autocontinue = 1
current = 0
param1 = 15.0 # minimum pitch
if seq == 0: # first waypoint to takeoff
current = 1
p = mavutil.mavlink.MAVLink_mission_item_message(
master.target_system, master.target_component, seq, frame,
mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, current, autocontinue,
param1, 0, 0, 0, lat, lon, altitude)
elif seq == len(waypoints) - 1: # last waypoint to land
p = mavutil.mavlink.MAVLink_mission_item_message(
master.target_system, master.target_component, seq, frame,
mavutil.mavlink.MAV_CMD_NAV_LAND, current, autocontinue, 0, 0,
0, 0, lat, lon, altitude)
else:
p = mavutil.mavlink.MAVLink_mission_item_message(
master.target_system, master.target_component, seq, frame,
mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, current, autocontinue, 0,
0, 0, 0, lat, lon, altitude)
wp.add(p)
# Send Waypoint to airframe
master.waypoint_clear_all_send()
master.waypoint_count_send(wp.count())
for i in range(wp.count()):
msg = master.recv_match(type=['MISSION_REQUEST'], blocking=True)
master.mav.send(wp.wp(msg.seq))
print('Sending waypoint {0}'.format(msg.seq))
msg = master.recv_match(type=['MISSION_ACK'], blocking=True) # OKAY
print(msg.type)
def add_points(wp, argagl, argstep, argmaxdelta, argrtlalt):
'''add more points for terrain following'''
wplist = []
wplist2 = []
for i in range(0, wp.count()):
wplist.append(wp.wp(i))
wplist[0].z = argagl
# add in RTL
wplist.append(wplist[0])
wplist2.append(wplist[0])
home = wp.wp(0)
home_ground = EleModel.GetElevation(home.x, home.y)
for i in range(1, len(wplist)):
prev = (wplist2[-1].x, wplist2[-1].y, wplist2[-1].z)
dist = cuav_util.gps_distance(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
bearing = cuav_util.gps_bearing(wplist2[-1].x, wplist2[-1].y, wplist[i].x, wplist[i].y)
print("dist=%u bearing=%u" % (dist, bearing))
while dist > argstep:
newpos = cuav_util.gps_newpos(prev[0], prev[1], bearing, argstep)
ground1 = EleModel.GetElevation(prev[0], prev[1])
ground2 = EleModel.GetElevation(newpos[0], newpos[1])
if ground2 == None:
ground2 = home_ground
agl = (home_ground + prev[2]) - ground2
if abs(agl - argagl) > argmaxdelta:
newwp = copy.copy(wplist2[-1])
newwp.x = newpos[0]
newwp.y = newpos[1]
newwp.z = (ground2 + argagl) - home_ground
wplist2.append(newwp)
print("Inserting at %u" % newwp.z)
prev = (newpos[0], newpos[1], newwp.z)
else:
prev = (newpos[0], newpos[1], wplist2[-1].z)
dist -= argstep
wplist2.append(wplist[i])
wplist2[-1].z = argrtlalt
wp2 = mavwp.MAVWPLoader()
for w in wplist2:
wp2.add(w)
wp2.save("newwp.txt")
return wp2
def __init__(self, mli):
# Keep pointer to main class
self.__mli = mli
self.__log = getLogger('Mission')
# Create pymavlink waypoint loader
self.wp = mavwp.MAVWPLoader()
# Initialize message counter to 1
self.seq = 1
# idk, but it's important. probably absolute vs relative altitude
self.__frame = mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT
# First command must be setHome TODO: figure out why
self.setHome()
self.__log.trace('New Mission object created')
def add_points(wp):
'''add more points for terrain following'''
wplist = []
wplist2 = []
for i in range(0, wp.count()):
wplist.append(wp.wp(i))
wplist[0].z = opts.agl
# add in RTL
wplist.append(wplist[0])
wplist2.append(wplist[0])
home = wp.wp(0)
home_ground = get_ground_alt(home.x, home.y)
for i in range(1, len(wplist)):
prev = (wplist2[-1].x, wplist2[-1].y, wplist2[-1].z)
dist = cuav_util.gps_distance(wplist2[-1].x, wplist2[-1].y,
wplist[i].x, wplist[i].y)
bearing = cuav_util.gps_bearing(wplist2[-1].x, wplist2[-1].y,
wplist[i].x, wplist[i].y)
print("dist=%u bearing=%u" % (dist, bearing))
while dist > opts.step:
newpos = cuav_util.gps_newpos(prev[0], prev[1], bearing, opts.step)
ground1 = get_ground_alt(prev[0], prev[1])
ground2 = get_ground_alt(newpos[0], newpos[1])
agl = (home_ground + prev[2]) - ground2
if abs(agl - opts.agl) > opts.maxdelta:
newwp = copy.copy(wplist2[-1])
newwp.x = newpos[0]
newwp.y = newpos[1]
newwp.z = (ground2 + opts.agl) - home_ground
wplist2.append(newwp)
print("Inserting at %u" % newwp.z)
prev = (newpos[0], newpos[1], newwp.z)
else:
prev = (newpos[0], newpos[1], wplist2[-1].z)
dist -= opts.step
wplist2.append(wplist[i])
wplist2[-1].z = opts.rtlalt
wp2 = mavwp.MAVWPLoader()
for w in wplist2:
wp2.add(w)
wp2.save("newwp.txt")
return wp2
def __init__(self, master, target_system, target_component):
#super(WPModule, self).__init__(mpstate, "wp", "waypoint handling", public = True)
self.wp_op = None
self.wp_requested = {}
self.wp_received = {}
self.wp_save_filename = None
self.wploader = mavwp.MAVWPLoader()
self.loading_waypoints = False
self.loading_waypoint_lasttime = time.time()
self.last_waypoint = 0
self.wp_period = mavutil.periodic_event(0.5)
self.undo_wp = None
self.undo_type = None
self.undo_wp_idx = -1
self.master = master
self.target_system = target_system
self.target_component = target_component
def _sendMission(self, mission_items):
wp = mavwp.MAVWPLoader()
seq = 1
for item in mission_items:
item.seq = seq
wp.add(item)
seq += 1
self.master.waypoint_clear_all_send()
self.master.waypoint_count_send(wp.count())
for i in range(wp.count()):
msg = self.master.recv_match(type=['MISSION_REQUEST'],
blocking=True)
self.master.mav.send(wp.wp(msg.seq))
print('Sending waypoint {0}'.format(msg.seq))
print("Mission uploaded")
def test_wp_is_loiter(self):
'''test is_loiter method'''
loader = mavwp.MAVWPLoader()
wp = self.makewp(1)
wp.command = mavutil.mavlink.MAV_CMD_NAV_LOITER_TURNS
loader.add(wp)
wp = self.makewp(2)
wp.command = mavutil.mavlink.MAV_CMD_NAV_WAYPOINT
loader.add(wp)
wp = self.makewp(2)
wp.command = mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH
loader.add(wp)
self.assertTrue(loader.wp_is_loiter(0))
self.assertFalse(loader.wp_is_loiter(1))
self.assertFalse(loader.wp_is_loiter(2))
def test_insert(self):
"""Test we can insert waypoints into mavwp"""
loader = mavwp.MAVWPLoader()
waypoints = self.make_wps()
loader.add(waypoints[0])
loader.add(waypoints[2])
self.assertEqual(loader.wp(0).seq, 0)
self.assertEqual(loader.wp(1).seq, 1)
self.assertEqual(loader.wp(0).z, 0)
self.assertEqual(loader.wp(1).z, 2)
loader.insert(1, waypoints[3])
self.assertEqual(loader.count(), 3)
self.assertEqual(loader.wp(0).seq, 0)
self.assertEqual(loader.wp(1).seq, 1)
self.assertEqual(loader.wp(2).seq, 2)
self.assertEqual(loader.wp(0).z, 0)
self.assertEqual(loader.wp(1).z, 3)
self.assertEqual(loader.wp(2).z, 2)
