diff --git a/MAVProxy/modules/lib/mission_item_protocol.py b/MAVProxy/modules/lib/mission_item_protocol.py new file mode 100644 index 0000000000..607f788d75 --- /dev/null +++ b/MAVProxy/modules/lib/mission_item_protocol.py @@ -0,0 +1,1076 @@ +#!/usr/bin/env python + +''' +base class for modules generally transfering items using the MISSION_ITEM protocol +''' + +import copy +import os +import re +import struct +import time + +from pymavlink import mavutil +from MAVProxy.modules.lib import mp_module +from MAVProxy.modules.lib import mp_util +if mp_util.has_wxpython: + from MAVProxy.modules.lib.mp_menu import MPMenuCallFileDialog + from MAVProxy.modules.lib.mp_menu import MPMenuItem + from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu + +try: + # py2 + from StringIO import StringIO as SIO +except ImportError: + # py3 + from io import BytesIO as SIO + + +class MissionItemProtocolModule(mp_module.MPModule): + def __init__(self, mpstate, name, description, **args): + super(MissionItemProtocolModule, self).__init__(mpstate, name, description, **args) + self.add_command(self.command_name(), + self.cmd_wp, + '%s management' % self.itemtype(), + self.completions()) + self.wp_op = None + self.wp_requested = {} + self.wp_received = {} + self.wp_save_filename = None + self.wploader_by_sysid = {} + 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.upload_start = None + self.last_get_home = time.time() + self.ftp_count = None + + if self.continue_mode and self.logdir is not None: + waytxt = os.path.join(mpstate.status.logdir, self.save_filename()) + if os.path.exists(waytxt): + self.wploader.load(waytxt) + print("Loaded %s from %s" % (self.itemstype(), waytxt)) + + self.init_gui_menus() + + def gui_menu_items(self): + return [ + MPMenuItem('Clear', 'Clear', '# %s clear' % self.command_name()), + MPMenuItem('List', 'List', '# %s list' % self.command_name()), + MPMenuItem( + 'Load', 'Load', '# %s load ' % self.command_name(), + handler=MPMenuCallFileDialog( + flags=('open',), + title='%s Load' % self.mission_type_string(), + wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')), + MPMenuItem( + 'Save', 'Save', '# %s save ' % self.command_name(), + handler=MPMenuCallFileDialog( + flags=('save', 'overwrite_prompt'), + title='%s Save' % self.mission_type_string(), + wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')), + MPMenuItem('Undo', 'Undo', '# %s undo' % self.command_name()), + ] + + def mission_type_string(self): + loader_class_string = str(self.loader_class()) + m = re.search("'([^']*)'", loader_class_string) + if m is None: + raise ValueError("Failed to match %s" % loader_class_string) + cname = m.group(1) + items = cname.split("_") + return items[-1] + + def init_gui_menus(self): + '''initialise menus for console and map''' + self.menu_added_console = False + self.menu_added_map = False + if not mp_util.has_wxpython: + return + + self.menu = MPMenuSubMenu( + self.mission_type_string(), + items=self.gui_menu_items() + ) + + def completions(self): + '''form up MAVProxy-style completion strings used for tab completi +on''' + cs = self.commands() + no_arguments = [] + command_argument_buckets = {} + for c in cs: + value = cs[c] + if type(value) == tuple: + (function, arguments) = value + args_string = " ".join(arguments) + if args_string not in command_argument_buckets: + command_argument_buckets[args_string] = [] + command_argument_buckets[args_string].append(c) + else: + no_arguments.append(c) + + ret = [] + if len(no_arguments): + ret.append("<" + "|".join(sorted(no_arguments)) + ">") + for k in command_argument_buckets: + ret.append("<" + "|".join(sorted(command_argument_buckets[k])) + "> " + k) + return ret + + def unload(self): + self.remove_command(self.command_name()) + if self.module('console') is not None and self.menu_added_console: + self.menu_added_console = False + self.module('console').remove_menu(self.menu) + if self.module('map') is not None and self.menu_added_map: + self.menu_added_map = False + self.module('map').remove_menu(self.menu) + super(MissionItemProtocolModule, self).unload() + + def create_loader(self): + c = self.loader_class() + return c() + + def last_change(self): + return self.wploader.last_change + + def check_have_list(self): + if self.last_change() == 0: + print("Please list %s items first" % self.command_name()) + return False + return True + + def index_from_0(self): + '''e.g. rally points etc are indexed from 1 from the user interface + perspective''' + return False + + def save_filename_base(self): + return self.itemstype().replace(" ", "-") + + def save_filename(self): + return self.save_filename_base() + ".txt" + + @property + def wploader(self): + '''per-sysid wploader''' + if self.target_system not in self.wploader_by_sysid: + self.wploader_by_sysid[self.target_system] = self.create_loader() + self.wploader_by_sysid[self.target_system].expected_count = 0 + return self.wploader_by_sysid[self.target_system] + + def good_item_num_to_manipulate(self, idx): + if idx > self.wploader.count(): + return False + if idx < 1: + return False + return True + + def item_num_to_offset(self, item_num): + if self.index_from_0(): + return item_num + return item_num - 1 + + def missing_wps_to_request(self): + ret = [] + tnow = time.time() + next_seq = self.wploader.count() + for i in range(5): + seq = next_seq+i + if seq+1 > self.wploader.expected_count: + continue + if seq in self.wp_requested and tnow - self.wp_requested[seq] < 2: + continue + ret.append(seq) + return ret + + def append(self, item): + '''append an item to the held item list''' + if not self.check_have_list(): + return + if type(item) == list: + for i in item: + self.wploader.add(i) + self.wploader.expected_count += 1 + else: + self.wploader.add(item) + self.wploader.expected_count += 1 + self.wploader.last_change = time.time() + self.wploader.reindex() + + def send_wp_requests(self, wps=None): + '''send some more WP requests''' + if wps is None: + wps = self.missing_wps_to_request() + tnow = time.time() + for seq in wps: + self.wp_requested[seq] = tnow + if self.settings.wp_use_mission_int: + method = self.master.mav.mission_request_int_send + else: + method = self.master.mav.mission_request_send + method(self.target_system, + self.target_component, + seq, + mission_type=self.mav_mission_type()) + + def cmd_status(self, args): + '''show status of wp download''' + if not self.check_have_list(): + return + try: + print("Have %u of %u %s" % ( + self.wploader.count()+len(self.wp_received), + self.wploader.expected_count, + self.itemstype())) + except Exception: + print("Have %u %s" % (self.wploader.count()+len(self.wp_received), self.itemstype())) + + def mavlink_packet(self, m): + '''handle an incoming mavlink packet''' + mtype = m.get_type() + if mtype in ['WAYPOINT_COUNT', 'MISSION_COUNT']: + if getattr(m, 'mission_type', 0) != self.mav_mission_type(): + return + if self.wp_op is None: + if self.wploader.expected_count != m.count: + self.console.writeln("Mission is stale") + else: + self.wploader.clear() + self.console.writeln("Requesting %u %s t=%s now=%s" % ( + m.count, + self.itemstype(), + time.asctime(time.localtime(m._timestamp)), + time.asctime())) + self.wploader.expected_count = m.count + self.send_wp_requests() + + elif mtype in ['WAYPOINT', 'MISSION_ITEM', 'MISSION_ITEM_INT'] and self.wp_op is not None: + if m.get_type() == 'MISSION_ITEM_INT': + if getattr(m, 'mission_type', 0) != self.mav_mission_type(): + # this is not a mission item, likely fence + return + # our internal structure assumes MISSION_ITEM''' + m = self.wp_from_mission_item_int(m) + if m.seq < self.wploader.count(): + # print("DUPLICATE %u" % m.seq) + return + if m.seq+1 > self.wploader.expected_count: + self.console.writeln("Unexpected %s number %u - expected %u" % (self.itemtype(), m.seq, self.wploader.count())) + self.wp_received[m.seq] = m + next_seq = self.wploader.count() + while next_seq in self.wp_received: + m = self.wp_received.pop(next_seq) + self.wploader.add(m) + next_seq += 1 + if self.wploader.count() != self.wploader.expected_count: + # print("m.seq=%u expected_count=%u" % (m.seq, self.wploader.expected_count)) + self.send_wp_requests() + return + if self.wp_op == 'list': + self.show_and_save(m.get_srcSystem()) + self.loading_waypoints = False + elif self.wp_op == "save": + self.save_waypoints(self.wp_save_filename) + self.wp_op = None + self.wp_requested = {} + self.wp_received = {} + + elif mtype in ["WAYPOINT_REQUEST", "MISSION_REQUEST"]: + self.process_waypoint_request(m, self.master) + + def idle_task(self): + '''handle missing waypoints''' + if self.wp_period.trigger(): + # cope with packet loss fetching mission + if (self.master is not None and + self.master.time_since('MISSION_ITEM') >= 2 and + self.wploader.count() < getattr(self.wploader, 'expected_count', 0)): + wps = self.missing_wps_to_request() + print("re-requesting %s %s" % (self.itemstype(), str(wps))) + self.send_wp_requests(wps) + + if self.module('console') is not None: + if not self.menu_added_console: + self.menu_added_console = True + self.module('console').add_menu(self.menu) + else: + self.menu_added_console = False + + if self.module('map') is not None: + if not self.menu_added_map: + self.menu_added_map = True + self.module('map').add_menu(self.menu) + else: + self.menu_added_map = False + + def has_location(self, cmd_id): + '''return True if a WP command has a location''' + if cmd_id in mavutil.mavlink.enums['MAV_CMD'].keys(): + cmd_enum = mavutil.mavlink.enums['MAV_CMD'][cmd_id] + # default to having location for older installs of pymavlink + # which don't have the attribute + return getattr(cmd_enum, 'has_location', True) + return False + + def wp_to_mission_item_int(self, wp): + '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as + MISSION_ITEM_INT to give cm level accuracy + ''' + if wp.get_type() == 'MISSION_ITEM_INT': + return wp + if self.has_location(wp.command): + p5 = int(wp.x*1.0e7) + p6 = int(wp.y*1.0e7) + else: + p5 = int(wp.x) + p6 = int(wp.y) + wp_int = mavutil.mavlink.MAVLink_mission_item_int_message( + wp.target_system, + wp.target_component, + wp.seq, + wp.frame, + wp.command, + wp.current, + wp.autocontinue, + wp.param1, + wp.param2, + wp.param3, + wp.param4, + p5, + p6, + wp.z, + wp.mission_type + ) + return wp_int + + def wp_from_mission_item_int(self, wp): + '''convert a MISSION_ITEM_INT to a MISSION_ITEM''' + if self.has_location(wp.command): + p5 = wp.x*1.0e-7 + p6 = wp.y*1.0e-7 + else: + p5 = wp.x + p6 = wp.y + wp2 = mavutil.mavlink.MAVLink_mission_item_message(wp.target_system, + wp.target_component, + wp.seq, + wp.frame, + wp.command, + wp.current, + wp.autocontinue, + wp.param1, + wp.param2, + wp.param3, + wp.param4, + p5, + p6, + wp.z, + wp.mission_type) + # preserve srcSystem as that is used for naming waypoint file + wp2._header.srcSystem = wp.get_srcSystem() + wp2._header.srcComponent = wp.get_srcComponent() + return wp2 + + def process_waypoint_request(self, m, master): + '''process a waypoint request from the master''' + if m.mission_type != self.mav_mission_type(): + return + print("Processing %s request: (%s)" % (self.itemtype(), str(m))) + if (m.target_system != self.settings.source_system or + m.target_component != self.settings.source_component): + # self.console.error("Mission request is not for me") + return + if (not self.loading_waypoints or + time.time() > self.loading_waypoint_lasttime + 10.0): + self.loading_waypoints = False + # self.console.error("not loading waypoints") + return + if m.seq >= self.wploader.count(): + self.console.error("Request for bad %s %u (max %u)" % + (self.itemtype, m.seq, self.wploader.count())) + return + wp = self.wploader.wp(m.seq) + wp.target_system = self.target_system + wp.target_component = self.target_component + if self.settings.wp_use_mission_int: + wp_send = self.wp_to_mission_item_int(wp) + else: + wp_send = wp + + if wp.mission_type != self.mav_mission_type(): + print("Wrong mission type in (%s)" % str(wp)) + + self.master.mav.send(wp_send) + + self.loading_waypoint_lasttime = time.time() + + # update the user on our progress: + self.mpstate.console.set_status(self.itemtype(), '%s %u/%u' % (self.itemtype(), m.seq, self.wploader.count()-1)) + + # see if the transfer is complete: + if m.seq == self.wploader.count() - 1: + self.loading_waypoints = False + print("Loaded %u %s in %.2fs" % ( + self.wploader.count(), + self.itemstype(), + time.time() - self.upload_start)) + self.console.writeln( + "Sent all %u %s" % + (self.wploader.count(), self.itemstype())) + + def send_all_waypoints(self): + return self.send_all_items() + + def send_all_items(self): + '''send all waypoints to vehicle''' + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.upload_start = time.time() + self.master.mav.mission_count_send( + self.target_system, + self.target_component, + self.wploader.count(), + mission_type=self.mav_mission_type()) + + def load_waypoints(self, filename): + '''load waypoints from a file''' + self.wploader.target_system = self.target_system + self.wploader.target_component = self.target_component + try: + # need to remove the leading and trailing quotes in filename + self.wploader.load(filename.strip('"')) + except Exception as msg: + print("Unable to load %s - %s" % (filename, msg)) + return + print("Loaded %u %s from %s" % (self.wploader.count(), self.itemstype(), filename)) + self.wploader.expected_count = self.wploader.count() + self.send_all_waypoints() + + def update_waypoints(self, filename, wpnum): + '''update waypoints from a file''' + self.wploader.target_system = self.target_system + self.wploader.target_component = self.target_component + try: + self.wploader.load(filename) + except Exception as msg: + print("Unable to load %s - %s" % (filename, msg)) + return + if self.wploader.count() == 0: + print("No %s found in %s" % (self.itemstype(), filename)) + return + if wpnum == -1: + print("Loaded %u updated %s from %s" % (self.wploader.count(), self.itemstype(), filename)) + elif wpnum >= self.wploader.count(): + print("Invalid %s number %u" % (self.itemtype(), wpnum)) + return + else: + print("Loaded updated %s %u from %s" % (self.itemtype(), wpnum, filename)) + + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + if wpnum == -1: + start = 0 + end = self.wploader.count()-1 + else: + start = wpnum + end = wpnum + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + start, + end, + self.mav_mission_type()) + + def save_waypoints(self, filename): + '''save waypoints to a file''' + try: + # need to remove the leading and trailing quotes in filename + self.wploader.save(filename.strip('"')) + except Exception as msg: + print("Failed to save %s - %s" % (filename, msg)) + return + print("Saved %u %s to %s" % (self.wploader.count(), self.itemstype(), filename)) + + def save_waypoints_csv(self, filename): + '''save waypoints to a file in a human readable CSV file''' + try: + # need to remove the leading and trailing quotes in filename + self.wploader.savecsv(filename.strip('"')) + except Exception as msg: + print("Failed to save %s - %s" % (filename, msg)) + return + print("Saved %u %s to CSV %s" % (self.wploader.count(), self.itemstype(), filename)) + + def cmd_move(self, args): + '''handle wp move''' + if len(args) != 1: + print("usage: wp move WPNUM") + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid %s number %u" % (self.itemtype(), idx)) + return + offset = self.item_num_to_offset(idx) + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + wp = self.wploader.wp(offset) + + # setup for undo + self.undo_wp = copy.copy(wp) + self.undo_wp_idx = idx + self.undo_type = "move" + + (lat, lon) = latlon + if (len(self.module_matching('terrain')) > 0 and + wp.frame == mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT and + self.settings.wpterrainadjust): + alt1 = self.module('terrain').ElevationModel.GetElevation(lat, lon) + alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y) + if alt1 is not None and alt2 is not None: + wp.z += alt1 - alt2 + wp.x = lat + wp.y = lon + + wp.target_system = self.target_system + wp.target_component = self.target_component + self.wploader.set(wp, offset) + self.wploader.last_change = time.time() + + print("Moving %s %u to %f, %f at %.1fm" % (self.itemtype(), idx, lat, lon, wp.z)) + + self.send_single_waypoint(offset) + + def send_single_waypoint(self, idx): + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + idx, + idx, + self.mav_mission_type() + ) + + def is_location_command(self, cmd): + '''see if cmd is a MAV_CMD with a latitude/longitude''' + mav_cmd = mavutil.mavlink.enums['MAV_CMD'] + if cmd not in mav_cmd: + return False + return getattr(mav_cmd[cmd], 'has_location', True) + + def is_location_wp(self, w): + '''see if w.command is a MAV_CMD with a latitude/longitude''' + if w.x == 0 and w.y == 0: + return False + return self.is_location_command(w.command) + + def cmd_movemulti(self, args, latlon=None): + '''handle wp move of multiple waypoints''' + if len(args) < 3: + print("usage: wp movemulti WPNUM WPSTART WPEND ") + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid move %s number %u" % (self.itemtype(), idx)) + return + wpstart = int(args[1]) + if not self.good_item_num_to_manipulate(wpstart): + print("Invalid start %s number %u" % (self.itemtype(), wpstart)) + return + wpend = int(args[2]) + if not self.good_item_num_to_manipulate(wpend): + print("Invalid end %s number %u" % (self.itemtype(), wpend)) + return + if idx < wpstart or idx > wpend: + print("WPNUM must be between WPSTART and WPEND") + return + + # optional rotation about center point + if len(args) > 3: + rotation = float(args[3]) + else: + rotation = 0 + + if latlon is None: + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + idx_offset = self.item_num_to_offset(idx) + wp = self.wploader.wp(idx_offset) + if not self.is_location_wp(wp): + print("WP must be a location command") + return + + (lat, lon) = latlon + distance = mp_util.gps_distance(wp.x, wp.y, lat, lon) + bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon) + + wpstart_offset = self.item_num_to_offset(wpstart) + wpend_offset = self.item_num_to_offset(wpend) + for wpnum in range(wpstart_offset, wpend_offset+1): + wp = self.wploader.wp(wpnum) + if wp is None or not self.is_location_wp(wp): + continue + (newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing, distance) + if wpnum != idx and rotation != 0: + # add in rotation + d2 = mp_util.gps_distance(lat, lon, newlat, newlon) + b2 = mp_util.gps_bearing(lat, lon, newlat, newlon) + (newlat, newlon) = mp_util.gps_newpos(lat, lon, b2+rotation, d2) + + if (len(self.module_matching('terrain')) > 0 and + wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT and + self.settings.wpterrainadjust): + alt1 = self.module('terrain').ElevationModel.GetElevation(newlat, newlon) + alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y) + if alt1 is not None and alt2 is not None: + wp.z += alt1 - alt2 + wp.x = newlat + wp.y = newlon + wp.target_system = self.target_system + wp.target_component = self.target_component + self.wploader.set(wp, wpnum) + + self.wploader.last_change = time.time() + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + wpstart_offset, + wpend_offset) + print("Moved %s %u:%u to %f, %f rotation=%.1f" % (self.itemstype(), wpstart, wpend, lat, lon, rotation)) + + def cmd_changealt(self, args): + '''handle wp change target alt of multiple waypoints''' + if not self.check_have_list(): + return + if len(args) < 2: + print("usage: %s changealt WPNUM NEWALT " % self.command_name()) + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid %s number %u" % (self.itemtype(), idx)) + return + newalt = float(args[1]) + if len(args) >= 3: + count = int(args[2]) + else: + count = 1 + if not self.good_item_num_to_manipulate(idx+count-1): + print("Invalid %s number %u" % (self.itemtype(), idx+count-1)) + return + + for wpnum in range(idx, idx+count): + offset = self.item_num_to_offset(wpnum) + wp = self.wploader.wp(offset) + if not self.wploader.is_location_command(wp.command): + continue + wp.z = newalt + wp.target_system = self.target_system + wp.target_component = self.target_component + print("new wp: (%s)" % str(wp)) + self.wploader.set(wp, offset) + + self.wploader.last_change = time.time() + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + offset, + offset, + mission_type=self.mav_mission_type()) + print("Changed alt for WPs %u:%u to %f" % (idx, idx+(count-1), newalt)) + + def fix_jumps(self, idx, delta): + # nothing by default as only waypoints need worry + pass + + def cmd_remove(self, args): + '''handle wp remove''' + if len(args) != 1: + print("usage: %s remove WPNUM" % self.command_name()) + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid %s number %u" % (self.itemtype(), idx)) + return + offset = self.item_num_to_offset(idx) + wp = self.wploader.wp(offset) + + # setup for undo + self.undo_wp = copy.copy(wp) + self.undo_wp_idx = idx + self.undo_type = "remove" + + self.wploader.remove(wp) + self.wploader.expected_count -= 1 + self.wploader.last_change = time.time() + self.fix_jumps(offset, -1) + self.send_all_waypoints() + print("Removed %s %u" % (self.itemtype(), idx)) + + def cmd_undo(self, args): + '''handle wp undo''' + if self.undo_wp_idx == -1 or self.undo_wp is None: + print("No undo information") + return + wp = self.undo_wp + if self.undo_type == 'move': + wp.target_system = self.target_system + wp.target_component = self.target_component + offset = self.item_num_to_offset(self.undo_wp_idx) + self.wploader.set(wp, offset) + self.wploader.last_change = time.time() + self.send_single_waypoint(offset) + print("Undid %s move" % self.itemtype()) + elif self.undo_type == 'remove': + offset = self.item_num_to_offset(self.undo_wp_idx) + self.wploader.insert(offset, wp) + self.wploader.expected_count += 1 + self.wploader.last_change = time.time() + self.fix_jumps(self.undo_wp_idx, 1) + self.send_all_waypoints() + print("Undid %s remove" % self.itemtype()) + else: + print("bad undo type") + self.undo_wp = None + self.undo_wp_idx = -1 + + def cmd_param(self, args): + '''handle wp parameter change''' + if len(args) < 2: + print("usage: wp param WPNUM PNUM ") + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid %s number %u" % (self.itemtype(), idx)) + return + offset = self.item_num_to_offset(idx) + wp = self.wploader.wp(offset) + param = [wp.param1, wp.param2, wp.param3, wp.param4] + pnum = int(args[1]) + if pnum < 1 or pnum > 4: + print("Invalid param number %u" % pnum) + return + + if len(args) == 2: + print("Param %u: %f" % (pnum, param[pnum-1])) + return + + param[pnum-1] = float(args[2]) + wp.param1 = param[0] + wp.param2 = param[1] + wp.param3 = param[2] + wp.param4 = param[3] + + wp.target_system = self.target_system + wp.target_component = self.target_component + self.wploader.set(wp, idx) + self.wploader.last_change = time.time() + print("Set param %u for %u to %f" % (pnum, idx, param[pnum-1])) + self.send_single_waypoint(idx) + + def cmd_clear(self, args): + self.master.mav.mission_clear_all_send( + self.target_system, + self.target_component, + mission_type=self.mav_mission_type()) + self.wploader.clear() + if getattr(self.wploader, 'expected_count', None) is not None: + self.wploader.expected_count = 0 + self.loading_waypoint_lasttime = time.time() + + def cmd_list(self, args): + self.wp_op = "list" + self.request_list_send() + + def cmd_load(self, args): + print("args: %s" % str(args)) + if len(args) != 1: + print("usage: %s load FILENAME" % self.command_name()) + return + self.load_waypoints(args[0]) + + def cmd_save(self, args): + if len(args) != 1: + print("usage: %s save " % self.command_name()) + return + self.wp_save_filename = args[0] + self.wp_op = "save" + self.request_list_send() + + def cmd_savecsv(self, args): + if len(args) != 1: + print("usage: wp savecsv ") + return + self.savecsv(args[0]) + + def cmd_savelocal(self, args): + if len(args) != 1: + print("usage: wp savelocal ") + return + self.wploader.save(args[0]) + + def cmd_show(self, args): + if len(args) != 1: + print("usage: wp show ") + return + self.wploader.load(args[0]) + + def cmd_update(self, args): + if not self.check_have_list(): + return + if len(args) < 1: + print("usage: %s update " % self.command_name()) + return + if len(args) == 2: + wpnum = int(args[1]) + else: + wpnum = -1 + self.update_waypoints(args[0], wpnum) + + def commands(self): + if not self.master.mavlink20(): + print("%s module not available; use old compat modules" % str(self.itemtype())) + return + return { + "clear": self.cmd_clear, + "list": self.cmd_list, + "load": (self.cmd_load, ["(FILENAME)"]), + "remove": self.cmd_remove, + "save": (self.cmd_save, ["(FILENAME)"]), + "savecsv": (self.cmd_savecsv, ["(FILENAME)"]), + "savelocal": self.cmd_savelocal, + "show": (self.cmd_show, ["(FILENAME)"]), + "status": self.cmd_status, + } + + def usage(self): + subcommands = "|".join(sorted(self.commands().keys())) + return "usage: %s <%s>" % (self.command_name(), subcommands) + + def cmd_wp(self, args): + '''waypoint commands''' + if len(args) < 1: + print(self.usage()) + return + + commands = self.commands() + if args[0] not in commands: + print(self.usage()) + return + + function = commands[args[0]] + if type(function) == tuple: + (function, function_arguments) = function + # TODO: do some argument validation here, remove same from + # cmd_* + + function(args[1:]) + + def pretty_enum_value(self, enum_name, enum_value): + if enum_name == "MAV_FRAME": + if enum_value == 0: + return "Abs" + elif enum_value == 1: + return "Local" + elif enum_value == 2: + return "Mission" + elif enum_value == 3: + return "Rel" + elif enum_value == 4: + return "Local ENU" + elif enum_value == 5: + return "Global (INT)" + elif enum_value == 10: + return "AGL" + ret = mavutil.mavlink.enums[enum_name][enum_value].name + ret = ret[len(enum_name)+1:] + return ret + + def csv_line(self, line): + '''turn a list of values into a CSV line''' + self.csv_sep = "," + return self.csv_sep.join(['"' + str(x) + '"' for x in line]) + + def pretty_parameter_value(self, value): + '''pretty parameter value''' + return value + + def savecsv(self, filename): + '''save waypoints to a file in human-readable CSV file''' + f = open(filename, mode='w') + headers = ["Seq", "Frame", "Cmd", "P1", "P2", "P3", "P4", "X", "Y", "Z"] + print(self.csv_line(headers)) + f.write(self.csv_line(headers) + "\n") + for w in self.wploader.wpoints: + if getattr(w, 'comment', None): + # f.write("# %s\n" % w.comment) + pass + out_list = [ + w.seq, + self.pretty_enum_value('MAV_FRAME', w.frame), + self.pretty_enum_value('MAV_CMD', w.command), + self.pretty_parameter_value(w.param1), + self.pretty_parameter_value(w.param2), + self.pretty_parameter_value(w.param3), + self.pretty_parameter_value(w.param4), + self.pretty_parameter_value(w.x), + self.pretty_parameter_value(w.y), + self.pretty_parameter_value(w.z), + ] + print(self.csv_line(out_list)) + f.write(self.csv_line(out_list) + "\n") + f.close() + + def fetch(self): + """Download wpts from vehicle (this operation is public to support other modules)""" + if self.wp_op is None: # If we were already doing a list or save, just restart the fetch without changing the operation # noqa + self.wp_op = "fetch" + self.request_list_send() + + def request_list_send(self): + self.master.mav.mission_request_list_send( + self.target_system, + self.target_component, + mission_type=self.mav_mission_type()) + + def wp_ftp_download(self, args): + '''Download wpts from vehicle with ftp''' + ftp = self.mpstate.module('ftp') + if ftp is None: + print("Need ftp module") + return + print("Fetching mission with ftp") + self.ftp_count = None + ftp.cmd_get([self.mission_ftp_name()], callback=self.ftp_callback, callback_progress=self.ftp_callback_progress) + + def ftp_callback_progress(self, fh, total_size): + '''progress callback from ftp fetch of mission''' + if self.ftp_count is None and total_size >= 10: + ofs = fh.tell() + fh.seek(0) + buf = fh.read(10) + fh.seek(ofs) + magic2, dtype, options, start, num_items = struct.unpack("= item_size: + mdata = data[:item_size] + data = data[item_size:] + msg = mavmsg.unpacker.unpack(mdata) + tlist = list(msg) + t = tlist[:] + for i in range(0, len(tlist)): + tlist[i] = t[mavmsg.orders[i]] + t = tuple(tlist) + w = mavmsg(*t) + w = self.wp_from_mission_item_int(w) + self.wploader.add(w) + self.show_and_save(self.target_system) + + def show_and_save(self, source_system): + '''display waypoints and save''' + for i in range(self.wploader.count()): + w = self.wploader.wp(i) + print("%u %u %.10f %.10f %f p1=%.1f p2=%.1f p3=%.1f p4=%.1f cur=%u auto=%u" % ( + w.command, w.frame, w.x, w.y, w.z, + w.param1, w.param2, w.param3, w.param4, + w.current, w.autocontinue)) + if self.logdir is not None: + fname = self.save_filename() + if source_system != 1: + fname = '%s_%u.txt' % (self.save_filename_base(), source_system) + waytxt = os.path.join(self.logdir, fname) + self.save_waypoints(waytxt) + print("Saved %s to %s" % (self.itemstype(), waytxt)) + + def wp_ftp_upload(self, args): + '''upload waypoints to vehicle with ftp''' + filename = args[0] + ftp = self.mpstate.module('ftp') + if ftp is None: + print("Need ftp module") + return + self.wploader.target_system = self.target_system + self.wploader.target_component = self.target_component + try: + # need to remove the leading and trailing quotes in filename + self.wploader.load(filename.strip('"')) + except Exception as msg: + print("Unable to load %s - %s" % (filename, msg)) + return + print("Loaded %u waypoints from %s" % (self.wploader.count(), filename)) + print("Sending mission with ftp") + + fh = SIO() + fh.write(struct.pack("") + return + + if 'draw_lines' not in self.mpstate.map_functions: + print("No map drawing available") + return + + self.mpstate.map_functions['draw_lines'](self.fence_draw_callback, + colour=draw_colour) + print("Drawing fence on map") + + def cmd_addcircle(self, args): + '''adds a circle to the map click position of specific type/radius''' + if not self.check_have_list(): + return + if len(args) < 2: + print("Need 2 arguments") + return + t = args[0] + radius = float(args[1]) + + latlon = self.mpstate.click_location + if latlon is None: + print("No click position available") + return + + if t in ["inclusion", "inc"]: + command = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION + elif t in ["exclusion", "exc"]: + command = mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION + else: + print("%s is not one of inclusion|exclusion" % t) + return + + m = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, # frame + command, # command + 0, # current + 0, # autocontinue + radius, # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + int(latlon[0] * 1e7), # x (latitude) + int(latlon[1] * 1e7), # y (longitude) + 0, # z (altitude) + self.mav_mission_type(), + ) + self.append(m) + self.send_all_items() + + def cmd_addpoly(self, args): + '''adds a number of waypoints equally spaced around a circle around + click point + + ''' + if not self.check_have_list(): + return + if len(args) < 1: + print("Need at least 1 argument (", "" "", "") + return + t = args[0] + count = 4 + radius = 20 + rotation = 0 + if len(args) > 1: + radius = float(args[1]) + if len(args) > 2: + count = int(args[2]) + if len(args) > 3: + rotation = float(args[3]) + + if count < 3: + print("Invalid count (%s)" % str(count)) + return + if radius <= 0: + print("Invalid radius (%s)" % str(radius)) + return + + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + + if t in ["inclusion", "inc"]: + command = mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION + elif t in ["exclusion", "exc"]: + command = mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION + else: + print("%s is not one of inclusion|exclusion" % t) + return + + items = [] + for i in range(0, count): + (lat, lon) = mavextra.gps_newpos(latlon[0], + latlon[1], + 360/float(count)*i + rotation, + radius) + + m = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, # frame + command, # command + 0, # current + 0, # autocontinue + count, # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + int(lat*1e7), # x (latitude) + int(lon*1e7), # y (longitude) + 0, # z (altitude) + self.mav_mission_type(), + ) + items.append(m) + + for m in items: + self.append(m) + self.send_all_items() + + def cmd_remove(self, args): + '''deny remove on fence - requires renumbering etc etc''' + print("remove is not currently supported for fence. Try removepolygon_point or removecircle") + if not self.check_have_list(): + return + + def removecircle(self, seq): + '''remove circle at offset seq''' + if not self.check_have_list(): + return + item = self.wploader.item(seq) + if item is None: + print("No item %s" % str(seq)) + return + + if not self.is_circle_item(item): + print("Item %u is not a circle" % seq) + return + self.wploader.remove(item) + self.wploader.expected_count -= 1 + self.wploader.last_change = time.time() + self.send_all_items() + + def is_circle_item(self, item): + return item.command in [ + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION, + mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION, + ] + + def removepolygon_point(self, polygon_start_seq, item_offset): + '''removes item at offset item_offset from the polygon starting at + polygon_start_seq''' + if not self.check_have_list(): + return + + items_to_set = [] + + first_item = self.wploader.item(polygon_start_seq) + if not self.is_polygon_item(first_item): + print("Item %u is not a polygon vertex" % polygon_start_seq) + return + original_count = int(first_item.param1) + if item_offset >= original_count: + print("Out-of-range point") + return + if original_count <= 3: + print("Too few points to remove one") + return + + dead_item_walking = self.wploader.item(polygon_start_seq + item_offset) + + # must reduce count in each of the polygons: + for i in range(int(first_item.param1)): + item = self.wploader.item(polygon_start_seq+i) + if int(item.param1) != original_count: + print("Invalid polygon starting at %u (count=%u), point %u (count=%u)" % + (polygon_start_seq, original_count, i, int(item.param1))) + return + item.param1 = item.param1 - 1 + items_to_set.append(item) + + for item in items_to_set: + w = item + self.wploader.set(w, w.seq) + + self.wploader.remove(dead_item_walking) + self.wploader.expected_count -= 1 + self.wploader.last_change = time.time() + self.send_all_items() + + def addpolygon_point(self, polygon_start_seq, item_offset): + '''adds item at offset item_offset into the polygon starting at + polygon_start_seq''' + + if not self.check_have_list(): + return + + items_to_set = [] + + first_item = self.wploader.item(polygon_start_seq) + if (first_item.command not in [ + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION + ]): + print("Item %u is not a polygon vertex" % polygon_start_seq) + return + original_count = int(first_item.param1) + if item_offset >= original_count: + print("Out-of-range point") + return + + # increase count in each of the polygon vertexes: + for i in range(int(first_item.param1)): + item = self.wploader.item(polygon_start_seq+i) + if int(item.param1) != original_count: + print("Invalid polygon starting at %u (count=%u), point %u (count=%u)" % + (polygon_start_seq, original_count, i, int(item.param1))) + return + item.param1 = item.param1 + 1 + items_to_set.append(item) + + for item in items_to_set: + w = item + self.wploader.set(w, w.seq) + + old_item = self.wploader.item(polygon_start_seq + item_offset) + new_item = copy.copy(old_item) + # reset latitude and longitude of new item to be half-way + # between it and the preceeding point + if item_offset == 0: + prev_item_offset = original_count-1 + else: + prev_item_offset = item_offset - 1 + prev_item = self.wploader.item(polygon_start_seq + prev_item_offset) + new_item.x = (old_item.x + prev_item.x)/2 + new_item.y = (old_item.y + prev_item.y)/2 + self.wploader.insert(polygon_start_seq + item_offset, new_item) + self.wploader.expected_count += 1 + self.wploader.last_change = time.time() + self.send_all_items() + + def is_polygon_item(self, item): + return item.command in [ + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION, + mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION, + ] + + def cmd_removepolygon(self, args): + self.removepolygon(int(args[0])) + + def removepolygon(self, seq): + '''remove polygon at offset seq''' + if not self.check_have_list(): + return + + first_item = self.wploader.item(seq) + if first_item is None: + print("Invalid item sequence number (%s)" % seq) + return + if not self.is_polygon_item(first_item): + print("Item %u is not a polygon vertex" % seq) + return + + items_to_remove = [] + for i in range(int(first_item.param1)): + item = self.wploader.item(seq+i) + if item is None: + print("No item %s" % str(i)) + return + if item.param1 != first_item.param1: + print("Invalid polygon starting at %u (count=%u), point %u (count=%u)" % + (seq, int(first_item.param1), i, int(item.param1))) + return + if not self.is_polygon_item(item): + print("Item %u point %u is not a polygon vertex" % (seq, i)) + return + items_to_remove.append(item) + + self.wploader.remove(items_to_remove) + self.wploader.expected_count -= len(items_to_remove) + self.wploader.last_change = time.time() + self.send_all_items() + + def cmd_movepolypoint(self, args): + '''moves item at offset item_offset in polygon starting at + polygon_start_seqence to map click point''' + if not self.check_have_list(): + return + + if len(args) < 2: + print("Need first polygon point and vertex offset") + return + polygon_start_seq = int(args[0]) + item_offset = int(args[1]) + first_item = self.wploader.item(polygon_start_seq) + if first_item is None: + print("No item at %u" % polygon_start_seq) + return + if not self.is_polygon_item(first_item): + print("Item %u is not a polygon vertex" % polygon_start_seq) + return + original_count = int(first_item.param1) + if item_offset >= original_count: + print("Out-of-range point") + return + + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + + moving_item = self.wploader.item(polygon_start_seq + item_offset) + moving_item.x = latlon[0] + moving_item.y = latlon[1] + if moving_item.get_type() == "MISSION_ITEM_INT": + moving_item.x *= 1e7 + moving_item.y *= 1e7 + + self.wploader.set(moving_item, moving_item.seq) + self.wploader.last_change = time.time() + + self.send_single_waypoint(moving_item.seq) + + def set_fence_enabled(self, do_enable): + '''Enable or disable fence''' + self.master.mav.command_long_send( + self.target_system, + self.target_component, + mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, + 0, + do_enable, + 0, + 0, + 0, + 0, + 0, + 0) + + def cmd_enable(self, args): + '''enable fence''' + self.set_fence_enabled(1) + + def cmd_disable(self, args): + '''disable fence''' + self.set_fence_enabled(0) + + def commands(self): + '''returns map from command name to handling function''' + ret = super(FenceModule, self).commands() + ret.update({ + 'addcircle': (self.cmd_addcircle, ["", "RADIUS"]), + 'addpoly': (self.cmd_addpoly, ["", "" "", ""]), + 'movepolypoint': (self.cmd_movepolypoint, ["POLY_FIRSTPOINT", "POINT_OFFSET"]), + 'enable': self.cmd_enable, + 'disable': self.cmd_disable, + 'draw': self.cmd_draw, + 'removepolygon': (self.cmd_removepolygon, ["POLY_FIRSTPOINT"]), + }) + return ret + + +def init(mpstate): + '''initialise module''' + return FenceModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_misseditor/mission_editor.py b/MAVProxy/modules/mavproxy_misseditor/mission_editor.py index 441246bc37..fde3f37d84 100644 --- a/MAVProxy/modules/mavproxy_misseditor/mission_editor.py +++ b/MAVProxy/modules/mavproxy_misseditor/mission_editor.py @@ -124,6 +124,7 @@ def run(self): elif event_type == me_event.MEE_WRITE_WPS: self.module('wp').wploader.clear() + self.module('wp').wploader.expected_count = event.get_arg("count") self.master().waypoint_count_send(event.get_arg("count")) self.mp_misseditor.num_wps_expected = event.get_arg("count") self.mp_misseditor.wps_received = {} diff --git a/MAVProxy/modules/mavproxy_rally.py b/MAVProxy/modules/mavproxy_rally.py new file mode 100644 index 0000000000..2f2eb32736 --- /dev/null +++ b/MAVProxy/modules/mavproxy_rally.py @@ -0,0 +1,143 @@ +'''rally point handling via MissionItemProtocol''' + +from MAVProxy.modules.lib import mission_item_protocol + +from pymavlink import mavutil +from pymavlink import mavwp + +from MAVProxy.modules.lib import mp_util + +if mp_util.has_wxpython: + from MAVProxy.modules.lib.mp_menu import MPMenuCallTextDialog + from MAVProxy.modules.lib.mp_menu import MPMenuItem + + +class RallyModule(mission_item_protocol.MissionItemProtocolModule): + + def __init__(self, mpstate): + '''initialise module; will raise AttributeError if pymavlink is too + old to use''' + # raise an attribute error if pymavlink is too old: + mavwp.MissionItemProtocol_Rally + super(RallyModule, self).__init__( + mpstate, + "rally", + "rally point management", + public=True) + + def command_name(self): + '''command-line command name''' + return "rally" + + def cmd_rally_add(self, args): + '''add a rally point at the last map click position''' + if not self.check_have_list(): + return + latlon = self.mpstate.click_location + if latlon is None: + print("No click position available") + return + + if len(args) < 1: + alt = self.settings.rallyalt + else: + alt = float(args[0]) + + m = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame + mavutil.mavlink.MAV_CMD_NAV_RALLY_POINT, # command + 0, # current + 0, # autocontinue + 0.0, # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + int(latlon[0] * 1e7), # x (latitude) + int(latlon[1] * 1e7), # y (longitude) + alt, # z (altitude) + self.mav_mission_type(), + ) + self.append(m) + self.send_all_items() + + def rally_point(self, i): + '''return an instance of the old mavlink rally_point message for the + item at offset i''' + i = self.wploader.item(i) + if i is None: + return None + lat = i.x + lng = i.y + alt = i.z + if i.get_type() == "MISSION_ITEM": + lat *= 1e7 + lng *= 1e7 + alt *= 100 + return mavutil.mavlink.MAVLink_rally_point_message( + i.target_system, + i.target_component, + i.seq, + self.wploader.count(), + lat, + lng, + alt, + 0, + 0, + 0) + + def rally_count(self): + '''return number of waypoints''' + return self.wploader.count() + + def commands(self): + '''returns map from command name to handling function''' + ret = super(RallyModule, self).commands() + ret.update({ + 'add': self.cmd_rally_add, + "move": self.cmd_move, # handled in parent class + "changealt": self.cmd_changealt, + "undo": self.cmd_undo, + }) + return ret + + def mission_ftp_name(self): + return "@MISSION/fence.dat" + + @staticmethod + def loader_class(): + return mavwp.MissionItemProtocol_Rally + + def mav_mission_type(self): + return mavutil.mavlink.MAV_MISSION_TYPE_RALLY + + def itemstype(self): + '''returns description of items in the plural''' + return 'rally items' + + def itemtype(self): + '''returns description of item''' + return 'rally item' + + def mavlink_packet(self, p): + super(RallyModule, self).mavlink_packet(p) + + def gui_menu_items(self): + ret = super(RallyModule, self).gui_menu_items() + ret.extend([ + MPMenuItem( + 'Add', 'Add', '# rally add ', + handler=MPMenuCallTextDialog( + title='Rally Altitude (m)', + default=100 + ) + ), + ]) + return ret + + +def init(mpstate): + '''initialise module''' + return RallyModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index d1bd911074..cc0c5c0d22 100644 --- a/MAVProxy/modules/mavproxy_wp.py +++ b/MAVProxy/modules/mavproxy_wp.py @@ -1,199 +1,82 @@ #!/usr/bin/env python '''waypoint command handling''' -import copy -import os -import struct +from MAVProxy.modules.lib import mission_item_protocol +from MAVProxy.modules.lib import mp_util + +from pymavlink import mavutil +from pymavlink import mavwp + import time -from pymavlink import mavutil, mavwp -from MAVProxy.modules.lib import mp_module -from MAVProxy.modules.lib import mp_util if mp_util.has_wxpython: - from MAVProxy.modules.lib.mp_menu import MPMenuCallFileDialog from MAVProxy.modules.lib.mp_menu import MPMenuCallTextDialog from MAVProxy.modules.lib.mp_menu import MPMenuItem - from MAVProxy.modules.lib.mp_menu import MPMenuSubMenu - -try: - # py2 - from StringIO import StringIO as SIO -except ImportError: - # py3 - from io import BytesIO as SIO -class WPModule(mp_module.MPModule): +class WPModule(mission_item_protocol.MissionItemProtocolModule): 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_by_sysid = {} - 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.upload_start = None - self.last_get_home = time.time() - self.add_command('wp', self.cmd_wp, 'waypoint management', - ["", # noqa - " (FILENAME)"]) - self.mission_ftp_name = "@MISSION/mission.dat" - self.ftp_count = None - # support for setting mission waypoint via command self.accepts_DO_SET_MISSION_CURRENT = {} # keyed by (sysid/compid) - if self.continue_mode and self.logdir is not 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) + def gui_menu_items(self): + ret = super(WPModule, self).gui_menu_items() + ret.extend([ + MPMenuItem('FTP', 'FTP', '# %s ftp' % self.command_name()), + MPMenuItem('Editor', 'Editor', '# wp editor'), + MPMenuItem( + 'Draw', 'Draw', '# wp draw ', + handler=MPMenuCallTextDialog( + title='Mission Altitude (m)', + default=100)), + MPMenuItem('Loop', 'Loop', '# wp loop'), + MPMenuItem( + 'Add Takeoff', 'Add Takeoff', '# wp add_takeoff ', + handler=MPMenuCallTextDialog( + title='Takeoff Altitude (m)', + default=20)), + MPMenuItem('Add Landing', 'Add Landing', '# wp add_landing'), + MPMenuItem('Add RTL', 'Add RTL', '# wp add_rtl'), + MPMenuItem('Add DO_LAND_START', 'Add DO_LAND_START', '# wp add_dls'), + MPMenuItem('Reset', 'Reset', '# wp set 0'), + ]) + return ret - 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('FTP', 'FTP', '# wp ftp'), - MPMenuItem( - 'Load', 'Load', '# wp load ', - handler=MPMenuCallFileDialog( - flags=('open',), - title='Mission Load', - wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')), - MPMenuItem( - 'Save', 'Save', '# wp save ', - handler=MPMenuCallFileDialog( - flags=('save', 'overwrite_prompt'), - title='Mission Save', - wildcard='MissionFiles(*.txt.*.wp,*.waypoints)|*.txt;*.wp;*.waypoints')), - MPMenuItem( - 'Draw', 'Draw', '# wp draw ', - handler=MPMenuCallTextDialog( - title='Mission Altitude (m)', - default=100)), - MPMenuItem('Undo', 'Undo', '# wp undo'), - MPMenuItem('Loop', 'Loop', '# wp loop'), - MPMenuItem( - 'Add Takeoff', 'Add Takeoff', '# wp add_takeoff ', - handler=MPMenuCallTextDialog( - title='Takeoff Altitude (m)', - default=20)), - MPMenuItem('Add Landing', 'Add Landing', '# wp add_landing'), - MPMenuItem('Add RTL', 'Add RTL', '# wp add_rtl'), - MPMenuItem('Add DO_LAND_START', 'Add DO_LAND_START', '# wp add_dls'), - MPMenuItem('Reset', 'Reset', '# wp set 0'), - ] - ) + def mission_ftp_name(self): + return "@MISSION/mission.dat" - @property - def wploader(self): - '''per-sysid wploader''' - if self.target_system not in self.wploader_by_sysid: - self.wploader_by_sysid[self.target_system] = mavwp.MAVWPLoader() - self.wploader_by_sysid[self.target_system].expected_count = 0 - return self.wploader_by_sysid[self.target_system] + def loader_class(self): + return mavwp.MAVWPLoader - def missing_wps_to_request(self): - ret = [] - tnow = time.time() - next_seq = self.wploader.count() - for i in range(5): - seq = next_seq+i - if seq+1 > self.wploader.expected_count: - continue - if seq in self.wp_requested and tnow - self.wp_requested[seq] < 2: - continue - ret.append(seq) - return ret + def mav_mission_type(self): + return mavutil.mavlink.MAV_MISSION_TYPE_MISSION - def is_quadplane(self): - Q_ENABLE = int(self.get_mav_param("Q_ENABLE", 0)) - return Q_ENABLE > 0 + def save_filename_base(self): + return 'way' - def send_wp_requests(self, wps=None): - '''send some more WP requests''' - if wps is None: - wps = self.missing_wps_to_request() - tnow = time.time() - for seq in wps: - self.wp_requested[seq] = tnow - if self.settings.wp_use_mission_int: - self.master.mav.mission_request_int_send(self.master.target_system, self.master.target_component, seq) - else: - self.master.mav.mission_request_send(self.master.target_system, self.master.target_component, seq) + def itemstype(self): + '''returns description of items in the plural''' + return 'waypoints' - def wp_status(self): - '''show status of wp download''' - try: - print("Have %u of %u waypoints" % (self.wploader.count()+len(self.wp_received), self.wploader.expected_count)) - except Exception: - print("Have %u waypoints" % (self.wploader.count()+len(self.wp_received))) + def itemtype(self): + '''returns description of item''' + return 'waypoint' + + def index_from_0(self): + # other similar user-visible interfaces start indexing + # user-modifiable items from 1. waypoints make index 0 + # visible to the user. + return True + + def command_name(self): + return "wp" def mavlink_packet(self, m): '''handle an incoming mavlink packet''' mtype = m.get_type() - if mtype in ['WAYPOINT_COUNT', 'MISSION_COUNT']: - if getattr(m, 'mission_type', 0) != 0: - # this is not a mission item, likely fence - return - if self.wp_op is None: - if self.wploader.expected_count != m.count: - self.console.writeln("Mission is stale") - else: - self.wploader.clear() - self.console.writeln("Requesting %u waypoints t=%s now=%s" % ( - m.count, - time.asctime(time.localtime(m._timestamp)), - time.asctime())) - self.wploader.expected_count = m.count - self.send_wp_requests() - elif mtype in ['WAYPOINT', 'MISSION_ITEM', 'MISSION_ITEM_INT'] and self.wp_op is not None: - if m.get_type() == 'MISSION_ITEM_INT': - if getattr(m, 'mission_type', 0) != 0: - # this is not a mission item, likely fence - return - # our internal structure assumes MISSION_ITEM''' - m = self.wp_from_mission_item_int(m) - if m.seq < self.wploader.count(): - # print("DUPLICATE %u" % m.seq) - return - if m.seq+1 > self.wploader.expected_count: - self.console.writeln("Unexpected waypoint number %u - expected %u" % (m.seq, self.wploader.count())) - self.wp_received[m.seq] = m - next_seq = self.wploader.count() - while next_seq in self.wp_received: - m = self.wp_received.pop(next_seq) - self.wploader.add(m) - next_seq += 1 - if self.wploader.count() != self.wploader.expected_count: - # print("m.seq=%u expected_count=%u" % (m.seq, self.wploader.expected_count)) - self.send_wp_requests() - return - if self.wp_op == 'list': - self.show_and_save(m.get_srcSystem()) - self.loading_waypoints = False - elif self.wp_op == "save": - self.save_waypoints(self.wp_save_filename) - self.wp_op = None - self.wp_requested = {} - self.wp_received = {} - - elif mtype in ["WAYPOINT_REQUEST", "MISSION_REQUEST"]: - self.process_waypoint_request(m, self.master) - - elif mtype in ["WAYPOINT_CURRENT", "MISSION_CURRENT"]: + if mtype in ["WAYPOINT_CURRENT", "MISSION_CURRENT"]: if m.seq != self.last_waypoint: self.last_waypoint = m.seq if self.settings.wpupdates: @@ -223,29 +106,9 @@ def mavlink_packet(self, m): elif m.result in [mavutil.mavlink.MAV_RESULT_ACCEPTED]: self.accepts_DO_SET_MISSION_CURRENT[key] = True - def idle_task(self): - '''handle missing waypoints''' - if self.wp_period.trigger(): - # cope with packet loss fetching mission - if (self.master is not None and - self.master.time_since('MISSION_ITEM') >= 2 and - self.wploader.count() < getattr(self.wploader, 'expected_count', 0)): - wps = self.missing_wps_to_request() - print("re-requesting WPs %s" % str(wps)) - self.send_wp_requests(wps) - if self.module('console') is not None: - if not self.menu_added_console: - self.menu_added_console = True - self.module('console').add_menu(self.menu) - else: - self.menu_added_console = False + super(WPModule, self).mavlink_packet(m) - if self.module('map') is not None: - if not self.menu_added_map: - self.menu_added_map = True - self.module('map').add_menu(self.menu) - else: - self.menu_added_map = False + def idle_task(self): if (self.master is not None and 'HOME_POSITION' not in self.master.messages and time.time() - self.last_get_home > 2): @@ -256,304 +119,9 @@ def idle_task(self): 0, 0, 0, 0, 0, 0, 0, 0) self.last_get_home = time.time() - def has_location(self, cmd_id): - '''return True if a WP command has a location''' - if cmd_id in mavutil.mavlink.enums['MAV_CMD'].keys(): - cmd_enum = mavutil.mavlink.enums['MAV_CMD'][cmd_id] - # default to having location for older installs of pymavlink - # which don't have the attribute - return getattr(cmd_enum, 'has_location', True) - return False - - def wp_to_mission_item_int(self, wp): - '''convert a MISSION_ITEM to a MISSION_ITEM_INT. We always send as - MISSION_ITEM_INT to give cm level accuracy - ''' - if wp.get_type() == 'MISSION_ITEM_INT': - return wp - if self.has_location(wp.command): - p5 = int(wp.x*1.0e7) - p6 = int(wp.y*1.0e7) - else: - p5 = int(wp.x) - p6 = int(wp.y) - wp_int = mavutil.mavlink.MAVLink_mission_item_int_message( - wp.target_system, - wp.target_component, - wp.seq, - wp.frame, - wp.command, - wp.current, - wp.autocontinue, - wp.param1, - wp.param2, - wp.param3, - wp.param4, - p5, - p6, - wp.z - ) - return wp_int - - def wp_from_mission_item_int(self, wp): - '''convert a MISSION_ITEM_INT to a MISSION_ITEM''' - if self.has_location(wp.command): - p5 = wp.x*1.0e-7 - p6 = wp.y*1.0e-7 - else: - p5 = wp.x - p6 = wp.y - wp2 = mavutil.mavlink.MAVLink_mission_item_message(wp.target_system, - wp.target_component, - wp.seq, - wp.frame, - wp.command, - wp.current, - wp.autocontinue, - wp.param1, - wp.param2, - wp.param3, - wp.param4, - p5, - p6, - wp.z) - # preserve srcSystem as that is used for naming waypoint file - wp2._header.srcSystem = wp.get_srcSystem() - wp2._header.srcComponent = wp.get_srcComponent() - return wp2 + super(WPModule, self).idle_task() - def process_waypoint_request(self, m, master): - '''process a waypoint request from the master''' - if (m.target_system != self.settings.source_system or - m.target_component != self.settings.source_component): - # self.console.error("Mission request is not for me") - return - if (not self.loading_waypoints or - time.time() > self.loading_waypoint_lasttime + 10.0): - self.loading_waypoints = False - # self.console.error("not loading waypoints") - return - if m.seq >= self.wploader.count(): - self.console.error("Request for bad waypoint %u (max %u)" % (m.seq, self.wploader.count())) - return - wp = self.wploader.wp(m.seq) - wp.target_system = self.target_system - wp.target_component = self.target_component - if self.settings.wp_use_mission_int: - wp_send = self.wp_to_mission_item_int(wp) - else: - wp_send = wp - self.master.mav.send(wp_send) - self.loading_waypoint_lasttime = time.time() - self.mpstate.console.set_status('Mission', 'Mission %u/%u' % (m.seq, self.wploader.count()-1)) - if m.seq == self.wploader.count() - 1: - self.loading_waypoints = False - print("Loaded %u waypoint in %.2fs" % (self.wploader.count(), time.time() - self.upload_start)) - self.console.writeln("Sent all %u waypoints" % self.wploader.count()) - - def send_all_waypoints(self): - '''send all waypoints to vehicle''' - self.upload_start = time.time() - self.master.waypoint_clear_all_send() - if self.wploader.count() == 0: - return - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.waypoint_count_send(self.wploader.count()) - - def load_waypoints(self, filename): - '''load waypoints from a file''' - self.wploader.target_system = self.target_system - self.wploader.target_component = self.target_component - try: - # need to remove the leading and trailing quotes in filename - self.wploader.load(filename.strip('"')) - except Exception as msg: - print("Unable to load %s - %s" % (filename, msg)) - return - print("Loaded %u waypoints from %s" % (self.wploader.count(), filename)) - self.send_all_waypoints() - - def update_waypoints(self, filename, wpnum): - '''update waypoints from a file''' - self.wploader.target_system = self.target_system - self.wploader.target_component = self.target_component - try: - self.wploader.load(filename) - except Exception as msg: - print("Unable to load %s - %s" % (filename, msg)) - return - if self.wploader.count() == 0: - print("No waypoints found in %s" % filename) - return - if wpnum == -1: - print("Loaded %u updated waypoints from %s" % (self.wploader.count(), filename)) - elif wpnum >= self.wploader.count(): - print("Invalid waypoint number %u" % wpnum) - return - else: - print("Loaded updated waypoint %u from %s" % (wpnum, filename)) - - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - if wpnum == -1: - start = 0 - end = self.wploader.count()-1 - else: - start = wpnum - end = wpnum - self.master.mav.mission_write_partial_list_send( - self.target_system, - self.target_component, - start, end) - - def save_waypoints(self, filename): - '''save waypoints to a file''' - try: - # need to remove the leading and trailing quotes in filename - self.wploader.save(filename.strip('"')) - except Exception as msg: - print("Failed to save %s - %s" % (filename, msg)) - return - print("Saved %u waypoints to %s" % (self.wploader.count(), filename)) - - def save_waypoints_csv(self, filename): - '''save waypoints to a file in a human readable CSV file''' - try: - # need to remove the leading and trailing quotes in filename - self.wploader.savecsv(filename.strip('"')) - except Exception as msg: - print("Failed to save %s - %s" % (filename, msg)) - return - print("Saved %u waypoints to CSV %s" % (self.wploader.count(), filename)) - - def cmd_wp_move(self, args): - '''handle wp move''' - if len(args) != 1: - print("usage: wp move WPNUM") - return - idx = int(args[0]) - if idx < 1 or idx > self.wploader.count(): - print("Invalid wp number %u" % idx) - return - latlon = self.mpstate.click_location - if latlon is None: - print("No map click position available") - return - wp = self.wploader.wp(idx) - - # setup for undo - self.undo_wp = copy.copy(wp) - self.undo_wp_idx = idx - self.undo_type = "move" - - (lat, lon) = latlon - if (len(self.module_matching('terrain')) > 0 and - wp.frame == mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT and - self.settings.wpterrainadjust): - alt1 = self.module('terrain').ElevationModel.GetElevation(lat, lon) - alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y) - if alt1 is not None and alt2 is not None: - wp.z += alt1 - alt2 - wp.x = lat - wp.y = lon - - wp.target_system = self.target_system - wp.target_component = self.target_component - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - idx, idx) - self.wploader.set(wp, idx) - print("Moved WP %u to %f, %f at %.1fm" % (idx, lat, lon, wp.z)) - - def is_location_command(self, cmd): - '''see if cmd is a MAV_CMD with a latitude/longitude''' - mav_cmd = mavutil.mavlink.enums['MAV_CMD'] - if cmd not in mav_cmd: - return False - return getattr(mav_cmd[cmd], 'has_location', True) - - def is_location_wp(self, w): - '''see if w.command is a MAV_CMD with a latitude/longitude''' - if w.x == 0 and w.y == 0: - return False - return self.is_location_command(w.command) - - def cmd_wp_movemulti(self, args, latlon=None): - '''handle wp move of multiple waypoints''' - if len(args) < 3: - print("usage: wp movemulti WPNUM WPSTART WPEND ") - return - idx = int(args[0]) - if idx < 1 or idx > self.wploader.count(): - print("Invalid wp number %u" % idx) - return - wpstart = int(args[1]) - if wpstart < 1 or wpstart > self.wploader.count(): - print("Invalid wp number %u" % wpstart) - return - wpend = int(args[2]) - if wpend < 1 or wpend > self.wploader.count(): - print("Invalid wp number %u" % wpend) - return - if idx < wpstart or idx > wpend: - print("WPNUM must be between WPSTART and WPEND") - return - - # optional rotation about center point - if len(args) > 3: - rotation = float(args[3]) - else: - rotation = 0 - - if latlon is None: - latlon = self.mpstate.click_location - if latlon is None: - print("No map click position available") - return - wp = self.wploader.wp(idx) - if not self.is_location_wp(wp): - print("WP must be a location command") - return - - (lat, lon) = latlon - distance = mp_util.gps_distance(wp.x, wp.y, lat, lon) - bearing = mp_util.gps_bearing(wp.x, wp.y, lat, lon) - - for wpnum in range(wpstart, wpend+1): - wp = self.wploader.wp(wpnum) - if wp is None or not self.is_location_wp(wp): - continue - (newlat, newlon) = mp_util.gps_newpos(wp.x, wp.y, bearing, distance) - if wpnum != idx and rotation != 0: - # add in rotation - d2 = mp_util.gps_distance(lat, lon, newlat, newlon) - b2 = mp_util.gps_bearing(lat, lon, newlat, newlon) - (newlat, newlon) = mp_util.gps_newpos(lat, lon, b2+rotation, d2) - - if (len(self.module_matching('terrain')) > 0 and - wp.frame != mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT and - self.settings.wpterrainadjust): - alt1 = self.module('terrain').ElevationModel.GetElevation(newlat, newlon) - alt2 = self.module('terrain').ElevationModel.GetElevation(wp.x, wp.y) - if alt1 is not None and alt2 is not None: - wp.z += alt1 - alt2 - wp.x = newlat - wp.y = newlon - wp.target_system = self.target_system - wp.target_component = self.target_component - self.wploader.set(wp, wpnum) - - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - wpstart, wpend+1) - print("Moved WPs %u:%u to %f, %f rotation=%.1f" % (wpstart, wpend, lat, lon, rotation)) - - def cmd_wp_move_rel_home(self, args, latlon=None): + def cmd_moverelhome(self, args, latlon=None): '''handle wp move to a point relative to home by dist/bearing''' if len(args) < 3: print("usage: wp moverelhome WPNUM dist bearing") @@ -588,452 +156,40 @@ def cmd_wp_move_rel_home(self, args, latlon=None): idx, idx+1) print("Moved WP %u %.1fm bearing %.1f from home" % (idx, dist, bearing)) - def cmd_wp_changealt(self, args): - '''handle wp change target alt of multiple waypoints''' - if len(args) < 2: - print("usage: wp changealt WPNUM NEWALT ") - return - idx = int(args[0]) - if idx < 1 or idx > self.wploader.count(): - print("Invalid wp number %u" % idx) - return - newalt = float(args[1]) - if len(args) >= 3: - count = int(args[2]) - else: - count = 1 - - for wpnum in range(idx, idx+count): - wp = self.wploader.wp(wpnum) - if not self.is_location_wp(wp): - continue - wp.z = newalt - wp.target_system = self.target_system - wp.target_component = self.target_component - self.wploader.set(wp, wpnum) - - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - idx, idx+count) - print("Changed alt for WPs %u:%u to %f" % (idx, idx+(count-1), newalt)) - - def cmd_wp_remove(self, args): - '''handle wp remove''' - if len(args) != 1: - print("usage: wp remove WPNUM") - return - idx = int(args[0]) - if idx < 0 or idx >= self.wploader.count(): - print("Invalid wp number %u" % idx) - return - wp = self.wploader.wp(idx) - - # setup for undo - self.undo_wp = copy.copy(wp) - self.undo_wp_idx = idx - self.undo_type = "remove" - - self.wploader.remove(wp) - self.fix_jumps(idx, -1) - self.send_all_waypoints() - print("Removed WP %u" % idx) - - def cmd_wp_undo(self): - '''handle wp undo''' - if self.undo_wp_idx == -1 or self.undo_wp is None: - print("No undo information") - return - wp = self.undo_wp - if self.undo_type == 'move': - wp.target_system = self.target_system - wp.target_component = self.target_component - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - self.undo_wp_idx, self.undo_wp_idx) - self.wploader.set(wp, self.undo_wp_idx) - print("Undid WP move") - elif self.undo_type == 'remove': - self.wploader.insert(self.undo_wp_idx, wp) - self.fix_jumps(self.undo_wp_idx, 1) - self.send_all_waypoints() - print("Undid WP remove") - else: - print("bad undo type") - self.undo_wp = None - self.undo_wp_idx = -1 + def commands(self): + ret = super(WPModule, self).commands() + + ret.update({ + 'add': self.cmd_add, + "changealt": self.cmd_changealt, + 'draw': self.cmd_draw, + 'editor': self.cmd_editor, + 'loop': self.cmd_loop, + "param": self.cmd_param, + "movemulti": self.cmd_movemulti, + "moverelhome": self.cmd_moverelhome, + 'set': self.cmd_set, + 'sethome': self.cmd_sethome, + 'slope': self.cmd_slope, + 'split': self.cmd_split, + "move": self.cmd_move, # handled in parent class + "add_takeoff": self.wp_add_takeoff, + "add_landing": self.wp_add_landing, + "add_rtl": self.wp_add_RTL, + "add_dls": self.wp_add_dls, + "ftp": self.wp_ftp_download, + "ftpload": self.wp_ftp_upload, + "update": (self.cmd_update, ["(FILENAME)"]), + "undo": self.cmd_undo, + }) - def cmd_wp_param(self, args): - '''handle wp parameter change''' - if len(args) < 2: - print("usage: wp param WPNUM PNUM ") - return - idx = int(args[0]) - if idx < 1 or idx > self.wploader.count(): - print("Invalid wp number %u" % idx) - return - wp = self.wploader.wp(idx) - param = [wp.param1, wp.param2, wp.param3, wp.param4] - pnum = int(args[1]) - if pnum < 1 or pnum > 4: - print("Invalid param number %u" % pnum) - return - - if len(args) == 2: - print("Param %u: %f" % (pnum, param[pnum-1])) - return - - param[pnum-1] = float(args[2]) - wp.param1 = param[0] - wp.param2 = param[1] - wp.param3 = param[2] - wp.param4 = param[3] - - wp.target_system = self.target_system - wp.target_component = self.target_component - self.loading_waypoints = True - self.loading_waypoint_lasttime = time.time() - self.master.mav.mission_write_partial_list_send(self.target_system, - self.target_component, - idx, idx) - self.wploader.set(wp, idx) - print("Set param %u for %u to %f" % (pnum, idx, param[pnum-1])) - - def cmd_clear(self, args): - '''clear waypoints''' - clear_type = mavutil.mavlink.MAV_MISSION_TYPE_MISSION - if len(args) > 0 and args[0] == "all": - clear_type = mavutil.mavlink.MAV_MISSION_TYPE_ALL - self.master.mav.mission_clear_all_send(self.target_system, self.target_component, clear_type) - self.wploader.clear() - self.loading_waypoint_lasttime = time.time() - - def cmd_wp(self, args): - '''waypoint commands''' - usage = "usage: wp " # noqa - if len(args) < 1: - print(usage) - return - - if args[0] == "load": - if len(args) != 2: - print("usage: wp load ") - return - self.load_waypoints(args[1]) - elif args[0] == "update": - if len(args) < 2: - print("usage: wp update ") - return - if len(args) == 3: - wpnum = int(args[2]) - else: - wpnum = -1 - self.update_waypoints(args[1], wpnum) - elif args[0] == "list": - self.wp_op = "list" - self.master.waypoint_request_list_send() - elif args[0] == "save": - if len(args) != 2: - print("usage: wp save ") - return - self.wp_save_filename = args[1] - self.wp_op = "save" - self.master.waypoint_request_list_send() - elif args[0] == "savecsv": - if len(args) != 2: - print("usage: wp savecsv ") - return - self.savecsv(args[1]) - elif args[0] == "savelocal": - if len(args) != 2: - print("usage: wp savelocal ") - return - self.wploader.save(args[1]) - elif args[0] == "show": - if len(args) != 2: - print("usage: wp show ") - return - self.wploader.load(args[1]) - elif args[0] == "move": - self.cmd_wp_move(args[1:]) - elif args[0] == "movemulti": - self.cmd_wp_movemulti(args[1:], None) - elif args[0] == "moverelhome": - self.cmd_wp_move_rel_home(args[1:], None) - elif args[0] == "changealt": - self.cmd_wp_changealt(args[1:]) - elif args[0] == "param": - self.cmd_wp_param(args[1:]) - elif args[0] == "remove": - self.cmd_wp_remove(args[1:]) - elif args[0] == "undo": - self.cmd_wp_undo() - elif args[0] == "set": - if len(args) != 2: - print("usage: wp set ") - return - # At time of writing MAVProxy sends to (1, 0) by default, - # but ArduPilot will respond from (1,1) by default -and - # that means COMMAND_ACK handling will fill - # self.accepts_DO_SET_MISSION_CURRENT for (1, 1) and we - # will not get that value here: - key = (self.target_system, self.target_component) - supports = self.accepts_DO_SET_MISSION_CURRENT.get(key, None) - # if we don't know, send both. If we do know, send only one. - # we "know" because we hook receipt of COMMAND_ACK. - - if self.settings.wp_use_waypoint_set_current or supports is False: - self.master.waypoint_set_current_send(int(args[1])) - else: - self.master.mav.command_long_send( - self.target_system, - self.target_component, - mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, - 0, - int(args[1]), 0, 0, 0, 0, 0, 0 - ) - - elif args[0] == "split": - self.cmd_split(args[1:]) - elif args[0] == "clear": - self.cmd_clear(args[1:]) - elif args[0] == "editor": - if self.module('misseditor'): - self.mpstate.functions.process_stdin("module reload misseditor", immediate=True) - else: - self.mpstate.functions.process_stdin("module load misseditor", immediate=True) - elif args[0] == "draw": - if 'draw_lines' not in self.mpstate.map_functions: - print("No map drawing available") - return - if self.get_home() is None: - print("Need home location - please run gethome") - return - if len(args) > 1: - self.settings.wpalt = int(args[1]) - self.mpstate.map_functions['draw_lines'](self.wp_draw_callback) - print("Drawing waypoints on map at altitude %d" % self.settings.wpalt) - elif args[0] == "sethome": - self.set_home_location() - elif args[0] == "loop": - self.wp_loop() - elif args[0] == "status": - self.wp_status() - elif args[0] == "slope": - self.wp_slope(args[1:]) - elif args[0] == "ftp": - self.wp_ftp_download() - elif args[0] == "ftpload": - if len(args) != 2: - print("usage: wp ftpload ") - return - self.wp_ftp_upload(args[1]) - elif args[0] == "add_takeoff": - self.wp_add_takeoff(args[1:]) - elif args[0] == "add_landing": - self.wp_add_landing() - elif args[0] == "add_rtl": - self.wp_add_RTL() - elif args[0] == "add_dls": - self.wp_add_dls() - else: - print(usage) - - def pretty_enum_value(self, enum_name, enum_value): - if enum_name == "MAV_FRAME": - if enum_value == 0: - return "Abs" - elif enum_value == 1: - return "Local" - elif enum_value == 2: - return "Mission" - elif enum_value == 3: - return "Rel" - elif enum_value == 4: - return "Local ENU" - elif enum_value == 5: - return "Global (INT)" - elif enum_value == 10: - return "AGL" - ret = mavutil.mavlink.enums[enum_name][enum_value].name - ret = ret[len(enum_name)+1:] return ret - def csv_line(self, line): - '''turn a list of values into a CSV line''' - self.csv_sep = "," - return self.csv_sep.join(['"' + str(x) + '"' for x in line]) - - def pretty_parameter_value(self, value): - '''pretty parameter value''' - return value - - def savecsv(self, filename): - '''save waypoints to a file in human-readable CSV file''' - f = open(filename, mode='w') - headers = ["Seq", "Frame", "Cmd", "P1", "P2", "P3", "P4", "X", "Y", "Z"] - print(self.csv_line(headers)) - f.write(self.csv_line(headers) + "\n") - for w in self.wploader.wpoints: - if getattr(w, 'comment', None): - # f.write("# %s\n" % w.comment) - pass - out_list = [ - w.seq, - self.pretty_enum_value('MAV_FRAME', w.frame), - self.pretty_enum_value('MAV_CMD', w.command), - self.pretty_parameter_value(w.param1), - self.pretty_parameter_value(w.param2), - self.pretty_parameter_value(w.param3), - self.pretty_parameter_value(w.param4), - self.pretty_parameter_value(w.x), - self.pretty_parameter_value(w.y), - self.pretty_parameter_value(w.z), - ] - print(self.csv_line(out_list)) - f.write(self.csv_line(out_list) + "\n") - f.close() - - def fetch(self): - """Download wpts from vehicle (this operation is public to support other modules)""" - if self.wp_op is None: # If we were already doing a list or save, just restart the fetch without changing the operation # noqa - self.wp_op = "fetch" - self.master.waypoint_request_list_send() - - def wp_ftp_download(self): - '''Download wpts from vehicle with ftp''' - ftp = self.mpstate.module('ftp') - if ftp is None: - print("Need ftp module") - return - print("Fetching mission with ftp") - self.ftp_count = None - ftp.cmd_get([self.mission_ftp_name], callback=self.ftp_callback, callback_progress=self.ftp_callback_progress) - - def ftp_callback_progress(self, fh, total_size): - '''progress callback from ftp fetch of mission''' - if self.ftp_count is None and total_size >= 10: - ofs = fh.tell() - fh.seek(0) - buf = fh.read(10) - fh.seek(ofs) - magic2, dtype, options, start, num_items = struct.unpack("= item_size: - mdata = data[:item_size] - data = data[item_size:] - msg = mavmsg.unpacker.unpack(mdata) - tlist = list(msg) - t = tlist[:] - for i in range(0, len(tlist)): - tlist[i] = t[mavmsg.orders[i]] - t = tuple(tlist) - w = mavmsg(*t) - w = self.wp_from_mission_item_int(w) - self.wploader.add(w) - self.show_and_save(self.target_system) - - def show_and_save(self, source_system): - '''display waypoints and save''' - for i in range(self.wploader.count()): - w = self.wploader.wp(i) - print("%u %u %.10f %.10f %f p1=%.1f p2=%.1f p3=%.1f p4=%.1f cur=%u auto=%u" % ( - w.command, w.frame, w.x, w.y, w.z, - w.param1, w.param2, w.param3, w.param4, - w.current, w.autocontinue)) - if self.logdir is not None: - fname = 'way.txt' - if source_system != 1: - fname = 'way_%u.txt' % source_system - waytxt = os.path.join(self.logdir, fname) - self.save_waypoints(waytxt) - print("Saved waypoints to %s" % waytxt) - - def wp_ftp_upload(self, filename): - '''upload waypoints to vehicle with ftp''' - ftp = self.mpstate.module('ftp') - if ftp is None: - print("Need ftp module") - return - self.wploader.target_system = self.target_system - self.wploader.target_component = self.target_component - try: - # need to remove the leading and trailing quotes in filename - self.wploader.load(filename.strip('"')) - except Exception as msg: - print("Unable to load %s - %s" % (filename, msg)) - return - print("Loaded %u waypoints from %s" % (self.wploader.count(), filename)) - print("Sending mission with ftp") - - fh = SIO() - fh.write(struct.pack(" 1: + self.settings.wpalt = int(args[1]) + self.mpstate.map_functions['draw_lines'](self.wp_draw_callback) + print("Drawing %s on map at altitude %d" % + (self.itemstype(), self.settings.wpalt)) + + def cmd_editor(self, args): + if self.module('misseditor'): + self.mpstate.functions.process_stdin("module reload misseditor", immediate=True) + else: + self.mpstate.functions.process_stdin("module load misseditor", immediate=True) + + def cmd_set(self, args): + if len(args) != 1: + print("usage: wp set ") + return + + wp_num = int(args[0]) + + # At time of writing MAVProxy sends to (1, 0) by default, + # but ArduPilot will respond from (1,1) by default -and + # that means COMMAND_ACK handling will fill + # self.accepts_DO_SET_MISSION_CURRENT for (1, 1) and we + # will not get that value here: + key = (self.target_system, self.target_component) + supports = self.accepts_DO_SET_MISSION_CURRENT.get(key, None) + # if we don't know, send both. If we do know, send only one. + # we "know" because we hook receipt of COMMAND_ACK. + + if self.settings.wp_use_waypoint_set_current or supports is False: + self.master.waypoint_set_current_send(wp_num) + else: + self.master.mav.command_long_send( + self.target_system, + self.target_component, + mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT, + 0, + wp_num, 0, 0, 0, 0, 0, 0 + ) + + def cmd_add(self, args): + '''add a NAV waypoint at the last map click position''' + if not self.check_have_list(): + return + latlon = self.mpstate.click_location + if latlon is None: + print("No click position available") + return + + if len(args) < 1: + alt = self.settings.wpalt + else: + alt = float(args[0]) + + m = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, # frame + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, # command + 0, # current + 0, # autocontinue + 0.0, # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + int(latlon[0] * 1e7), # x (latitude) + int(latlon[1] * 1e7), # y (longitude) + alt, # z (altitude) + self.mav_mission_type(), + ) + self.append(m) + self.send_all_items() + + def cmd_loop(self, args): '''close the loop on a mission''' loader = self.wploader if loader.count() < 2: @@ -1138,8 +375,14 @@ def wp_loop(self): self.master.waypoint_count_send(self.wploader.count()) print("Closed loop on mission") + def is_quadplane(self): + Q_ENABLE = int(self.get_mav_param("Q_ENABLE", 0)) + return Q_ENABLE > 0 + def wp_add_takeoff(self, args): '''add a takeoff as first mission item''' + if not self.check_have_list(): + return latlon = self.mpstate.click_location if latlon is None: print("No position chosen") @@ -1166,8 +409,10 @@ def wp_add_takeoff(self, args): self.wploader.insert(1, wp) self.send_all_waypoints() - def wp_add_landing(self): + def wp_add_landing(self, args): '''add a landing as last mission item''' + if not self.check_have_list(): + return latlon = self.mpstate.click_location if latlon is None: print("No position chosen") @@ -1184,8 +429,10 @@ def wp_add_landing(self): self.wploader.add(wp) self.send_all_waypoints() - def wp_add_RTL(self): + def wp_add_RTL(self, args): '''add a RTL as last mission item''' + if not self.check_have_list(): + return wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, @@ -1194,8 +441,10 @@ def wp_add_RTL(self): self.wploader.add(wp) self.send_all_waypoints() - def wp_add_dls(self): + def wp_add_dls(self, args): '''add a DO_LAND_START as last mission item''' + if not self.check_have_list(): + return latlon = self.mpstate.click_location if latlon is None: print("No position chosen") @@ -1208,7 +457,7 @@ def wp_add_dls(self): self.wploader.add(wp) self.send_all_waypoints() - def set_home_location(self): + def cmd_sethome(self, args): '''set home location from last map click''' latlon = self.mpstate.click_location if latlon is None: @@ -1320,6 +569,7 @@ def cmd_split(self, args): alt_avg * 1e-2, # z (altitude) ) self.wploader.insert(wp.seq, new_wp) + self.wploader.expected_count += 1 self.fix_jumps(wp.seq, 1) self.send_all_waypoints()