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)