diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index 7dff43a88e..641e9a9eae 100644 --- a/MAVProxy/mavproxy.py +++ b/MAVProxy/mavproxy.py @@ -356,6 +356,62 @@ def module(self, name): return self.public_modules[name] return None + def load_module(self, modname, quiet=False, **kwargs): + '''load a module''' + modpaths = ['MAVProxy.modules.mavproxy_%s' % modname, modname] + for (m,pm) in mpstate.modules: + if m.name == modname and not modname in mpstate.multi_instance: + if not quiet: + print("module %s already loaded" % modname) + # don't report an error + return True + ex = None + for modpath in modpaths: + try: + m = import_package(modpath) + reload(m) + module = m.init(mpstate, **kwargs) + if isinstance(module, mp_module.MPModule): + mpstate.modules.append((module, m)) + if not quiet: + if kwargs: + print("Loaded module %s with kwargs = %s" % (modname, kwargs)) + else: + print("Loaded module %s" % (modname,)) + return True + else: + ex = "%s.init did not return a MPModule instance" % modname + break + except ImportError as msg: + ex = msg + if mpstate.settings.moddebug > 1: + print(get_exception_stacktrace(ex)) + help_traceback = "" + if mpstate.settings.moddebug < 3: + help_traceback = " Use 'set moddebug 3' in the MAVProxy console to enable traceback" + print("Failed to load module: %s.%s" % (ex, help_traceback)) + return False + + def unload_module(self, modname): + '''unload a module''' + for (m,pm) in mpstate.modules: + if m.name == modname: + if hasattr(m, 'unload'): + t = threading.Thread(target=lambda : m.unload(), name="unload %s" % modname) + t.start() + t.join(timeout=5) + if t.is_alive(): + print("unload on module %s did not complete" % m.name) + mpstate.modules.remove((m,pm)) + return False + mpstate.modules.remove((m,pm)) + if modname in mpstate.public_modules: + del mpstate.public_modules[modname] + print("Unloaded module %s" % modname) + return True + print("Unable to find module %s" % modname) + return False + def master(self, target_sysid = -1): '''return the currently chosen mavlink master object''' if len(self.mav_master) == 0: @@ -513,62 +569,6 @@ def get_exception_stacktrace(e): return ret return traceback.format_exc(e) -def load_module(modname, quiet=False, **kwargs): - '''load a module''' - modpaths = ['MAVProxy.modules.mavproxy_%s' % modname, modname] - for (m,pm) in mpstate.modules: - if m.name == modname and not modname in mpstate.multi_instance: - if not quiet: - print("module %s already loaded" % modname) - # don't report an error - return True - ex = None - for modpath in modpaths: - try: - m = import_package(modpath) - reload(m) - module = m.init(mpstate, **kwargs) - if isinstance(module, mp_module.MPModule): - mpstate.modules.append((module, m)) - if not quiet: - if kwargs: - print("Loaded module %s with kwargs = %s" % (modname, kwargs)) - else: - print("Loaded module %s" % (modname,)) - return True - else: - ex = "%s.init did not return a MPModule instance" % modname - break - except ImportError as msg: - ex = msg - if mpstate.settings.moddebug > 1: - print(get_exception_stacktrace(ex)) - help_traceback = "" - if mpstate.settings.moddebug < 3: - help_traceback = " Use 'set moddebug 3' in the MAVProxy console to enable traceback" - print("Failed to load module: %s.%s" % (ex, help_traceback)) - return False - -def unload_module(modname): - '''unload a module''' - for (m,pm) in mpstate.modules: - if m.name == modname: - if hasattr(m, 'unload'): - t = threading.Thread(target=lambda : m.unload(), name="unload %s" % modname) - t.start() - t.join(timeout=5) - if t.is_alive(): - print("unload on module %s did not complete" % m.name) - mpstate.modules.remove((m,pm)) - return False - mpstate.modules.remove((m,pm)) - if modname in mpstate.public_modules: - del mpstate.public_modules[modname] - print("Unloaded module %s" % modname) - return True - print("Unable to find module %s" % modname) - return False - def cmd_module(args): '''module commands''' usage = "usage: module " @@ -588,7 +588,7 @@ def cmd_module(args): return (modname, kwargs) = generate_kwargs(args[1]) try: - load_module(modname, **kwargs) + mpstate.load_module(modname, **kwargs) except TypeError as ex: print(ex) print("%s module does not support keyword arguments"% modname) @@ -605,7 +605,7 @@ def cmd_module(args): if pmodule is None: print("Module %s not loaded" % modname) return - if unload_module(modname): + if mpstate.unload_module(modname): import zipimport try: reload(pmodule) @@ -613,7 +613,7 @@ def cmd_module(args): clear_zipimport_cache() reload(pmodule) try: - if load_module(modname, quiet=True, **kwargs): + if mpstate.load_module(modname, quiet=True, **kwargs): print("Reloaded module %s" % modname) except TypeError: print("%s module does not support keyword arguments" % modname) @@ -622,7 +622,7 @@ def cmd_module(args): print("usage: module unload ") return modname = os.path.basename(args[1]) - unload_module(modname) + mpstate.unload_module(modname) else: print(usage) @@ -1052,7 +1052,7 @@ def periodic_tasks(): # also see if the module should be unloaded: if m.needs_unloading: - unload_module(m.name) + mpstate.unload_module(m.name) def main_loop(): '''main processing loop''' @@ -1334,7 +1334,7 @@ def set_mav_version(mav10, mav20, autoProtocol, mavversionArg): if opts.speech: # start the speech-dispatcher early, so it doesn't inherit any ports from # modules/mavutil - load_module('speech') + mpstate.load_module('speech') serial_list = mavutil.auto_detect_serial(preferred_list=preferred_ports) serial_list.sort(key=lambda x: x.device) @@ -1378,7 +1378,7 @@ def quit_handler(signum = None, frame = None): for sig in fatalsignals: signal.signal(sig, quit_handler) - load_module('link', quiet=True) + mpstate.load_module('link', quiet=True) mpstate.settings.source_system = opts.SOURCE_SYSTEM mpstate.settings.source_component = opts.SOURCE_COMPONENT @@ -1445,7 +1445,7 @@ def quit_handler(signum = None, frame = None): standard_modules = opts.default_modules.split(',') for m in standard_modules: if m: - load_module(m, quiet=True) + mpstate.load_module(m, quiet=True) if platform.system() != 'Windows': if opts.console: diff --git a/MAVProxy/modules/lib/mission_item_protocol.py b/MAVProxy/modules/lib/mission_item_protocol.py new file mode 100644 index 0000000000..652318f4ea --- /dev/null +++ b/MAVProxy/modules/lib/mission_item_protocol.py @@ -0,0 +1,1070 @@ +#!/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.upload_start = 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 + 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() + 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): + 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"] + 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), + ] + 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 + 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("", - " (FILENAME)"]) - - self.have_list = False - - if self.continue_mode and self.logdir is not None: - fencetxt = os.path.join(self.logdir, 'fence.txt') - if os.path.exists(fencetxt): - self.fenceloader.load(fencetxt) - self.have_list = True - print("Loaded fence from %s" % fencetxt) - - self.menu_added_console = False - self.menu_added_map = False - if mp_util.has_wxpython: - self.menu = MPMenuSubMenu('Fence', - items=[MPMenuItem('Clear', 'Clear', '# fence clear'), - MPMenuItem('List', 'List', '# fence list'), - MPMenuItem('Load', 'Load', '# fence load ', - handler=MPMenuCallFileDialog(flags=('open',), - title='Fence Load', - wildcard='FenceFiles(*.txt,*.fen)|*.txt;*.fen')), - MPMenuItem('Save', 'Save', '# fence save ', - handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'), - title='Fence Save', - wildcard='FenceFiles(*.txt,*.fen)|*.txt;*.fen')), - MPMenuItem('Draw', 'Draw', '# fence draw')]) - - @property - def fenceloader(self): - '''fence loader by sysid''' - if not self.target_system in self.fenceloader_by_sysid: - self.fenceloader_by_sysid[self.target_system] = mavwp.MAVFenceLoader() - return self.fenceloader_by_sysid[self.target_system] - - def idle_task(self): - '''called on idle''' - 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 gui_menu_items(self): + ret = super(FenceModule, self).gui_menu_items() + ret.extend([ + MPMenuItem( + 'Add Inclusion Circle', 'Add Inclusion Circle', '# fence addcircle inc ', + handler=MPMenuCallTextDialog( + title='Radius (m)', + default=500 + ) + ), + MPMenuItem( + 'Add Exclusion Circle', 'Add Exclusion Circle', '# fence addcircle exc ', + handler=MPMenuCallTextDialog( + title='Radius (m)', + default=500 + ) + ), + MPMenuItem( + 'Draw Inclusion Poly', 'Draw Inclusion Poly', '# fence draw inc ', + ), + MPMenuItem( + 'Draw Exclusion Poly', 'Draw Exclusion Poly', '# fence draw exc ', + ), + ]) + return ret + + def command_name(self): + '''command-line command name''' + return "fence" + + # def fence_point(self, i): + # return self.wploader.fence_point(i) + + def count(self): + '''return number of waypoints''' + return self.wploader.count() + + def circles_of_type(self, t): + '''return a list of Circle fences of a specific type - a single + MISSION_ITEM''' + ret = [] + loader = self.wploader + for i in range(0, loader.count()): + p = loader.item(i) + if p is None: + print("Bad loader item (%u)" % i) + return [] + if p.command != t: + continue + ret.append(p) + return ret + + def inclusion_circles(self): + '''return a list of Circle inclusion fences - a single MISSION_ITEM each''' + return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION) + + def exclusion_circles(self): + '''return a list of Circle exclusion fences - a single MISSION_ITEM each''' + return self.circles_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION) + + def polygons_of_type(self, t): + '''return a list of polygon fences of a specific type - each a list of + items''' + ret = [] + loader = self.wploader + state_outside = 99 + state_inside = 98 + current_polygon = [] + current_expected_length = 0 + state = state_outside + for i in range(0, loader.count()): + p = loader.item(i) + if p is None: + print("Bad loader item (%u)" % i) + return [] + if p.command == t: + # sanity checks: + if p.param1 < 3: + print("Bad vertex count (%u) in seq=%u" % (p.param1, p.seq)) + continue + if state == state_outside: + # starting a new polygon + state = state_inside + current_expected_length = p.param1 + current_polygon = [] + current_polygon.append(p) + continue + if state == state_inside: + # if the count is different and we're in state + # inside then the current polygon is invalid. + # Discard it. + if p.param1 != current_expected_length: + print("Short polygon found, discarding") + current_expected_length = p.param1 + current_polygon = [] + current_polygon.append(p) + continue + current_polygon.append(p) + if len(current_polygon) == current_expected_length: + ret.append(current_polygon) + state = state_outside + continue + print("Unknown state (%s)" % str(state)) + else: + if state == state_inside: + if len(current_polygon) != current_expected_length: + print("Short polygon found") + else: + ret.append(current_polygon) + state = state_outside + continue + if state == state_outside: + continue + print("Unknown state (%s)" % str(state)) + return ret + + def inclusion_polygons(self): + '''return a list of polygon inclusion fences - each a list of items''' + return self.polygons_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION) + + def exclusion_polygons(self): + '''return a list of polygon exclusion fences - each a list of items''' + return self.polygons_of_type(mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION) + + @staticmethod + def loader_class(): + return mavwp.MissionItemProtocol_Fence + + def mav_mission_type(self): + return mavutil.mavlink.MAV_MISSION_TYPE_FENCE + + def itemstype(self): + '''returns description of items in the plural''' + return 'fence items' + + def itemtype(self): + '''returns description of item''' + return 'fence item' + + def handle_sys_status(self, m): + '''function to handle SYS_STATUS packets, used by both old and new module''' + bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + present = ((m.onboard_control_sensors_present & bits) == bits) + if self.present is False and present is True: + self.say("fence present") + elif self.present is True and present is False: + self.say("fence removed") + self.present = present + + enabled = ((m.onboard_control_sensors_enabled & bits) == bits) + if self.enabled is False and enabled is True: + self.say("fence enabled") + elif self.enabled is True and enabled is False: + self.say("fence disabled") + self.enabled = enabled + + healthy = ((m.onboard_control_sensors_health & bits) == bits) + if self.healthy is False and healthy is True: + self.say("fence OK") + elif self.healthy is True and healthy is False: + self.say("fence breach") + self.healthy = healthy + + # console output for fence: + if not self.present: + self.console.set_status('Fence', 'FEN', row=0, fg='black') + elif self.enabled is False: + self.console.set_status('Fence', 'FEN', row=0, fg='grey') + elif self.enabled is True and self.healthy is True: + self.console.set_status('Fence', 'FEN', row=0, fg='green') + elif self.enabled is True and self.healthy is False: + self.console.set_status('Fence', 'FEN', row=0, fg='red') def mavlink_packet(self, m): - '''handle and incoming mavlink packet''' - if self.sysid != 0 and (self.sysid != m.get_srcSystem() or self.compid != m.get_srcComponent()): - # ignore messages from other components we haven't locked on to - return - if m.get_type() == "FENCE_STATUS": - self.last_fence_breach = m.breach_time - self.last_fence_status = m.breach_status - self.compid = m.get_srcComponent() - self.sysid = m.get_srcSystem() - elif m.get_type() in ['SYS_STATUS']: - bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE - - present = ((m.onboard_control_sensors_present & bits) == bits) - if self.present == False and present == True: - self.say("fence present") - self.compid = m.get_srcComponent() - self.sysid = m.get_srcSystem() - elif self.present == True and present == False: - self.say("fence removed") - self.present = present - - enabled = ((m.onboard_control_sensors_enabled & bits) == bits) - if self.enabled == False and enabled == True: - self.say("fence enabled") - self.compid = m.get_srcComponent() - self.sysid = m.get_srcSystem() - elif self.enabled == True and enabled == False: - self.say("fence disabled") - self.enabled = enabled - - healthy = ((m.onboard_control_sensors_health & bits) == bits) - if self.healthy == False and healthy == True: - self.say("fence OK") - elif self.healthy == True and healthy == False: - self.say("fence breach") - self.healthy = healthy - - #console output for fence: - if not self.present: - self.console.set_status('Fence', 'FEN', row=0, fg='black') - elif self.enabled == False: - self.console.set_status('Fence', 'FEN', row=0, fg='grey') - elif self.enabled == True and self.healthy == True: - self.console.set_status('Fence', 'FEN', row=0, fg='green') - elif self.enabled == True and self.healthy == False: - self.console.set_status('Fence', 'FEN', row=0, fg='red') + if m.get_type() == 'SYS_STATUS': + self.handle_sys_status(m) + super(FenceModule, self).mavlink_packet(m) - def set_fence_enabled(self, do_enable): - '''Enable or disable fence''' - self.master.mav.command_long_send( + def fence_draw_callback(self, points): + '''callback from drawing a fence''' + if len(points) < 3: + print("Fence draw cancelled") + return + items = [] + for p in points: + m = mavutil.mavlink.MAVLink_mission_item_int_message( + self.target_system, + self.target_component, + 0, # seq + mavutil.mavlink.MAV_FRAME_GLOBAL, # frame + self.drawing_fence_type, # command + 0, # current + 0, # autocontinue + len(points), # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + int(p[0]*1e7), # x (latitude) + int(p[1]*1e7), # y (longitude) + 0, # z (altitude) + self.mav_mission_type(), + ) + items.append(m) + + self.append(items) + self.send_all_items() + self.wploader.last_change = time.time() + + def cmd_draw(self, args): + '''convenience / compatability / slow learner command to work like the + old module - i.e. a single inclusion polyfence''' + # TODO: emit this only if there actually are complex fences: + if not self.check_have_list(): + return + + if len(args) == 0: + print("WARNING! You want 'fence draw inc' or 'fence draw exc'") + return + if args[0] in ("inc", "inclusion"): + self.drawing_fence_type = mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION + draw_colour = (128, 128, 255) + elif args[0] in ("exc", "exclusion"): + self.drawing_fence_type = mavutil.mavlink.MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION + draw_colour = (255, 128, 128) + else: + print("fence draw ") + 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, - mavutil.mavlink.MAV_CMD_DO_FENCE_ENABLE, 0, - do_enable, 0, 0, 0, 0, 0, 0) + 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_fence_move(self, args): - '''handle fencepoint move''' - if len(args) < 1: - print("Usage: fence move FENCEPOINTNUM") + 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 not self.have_list: - print("Please list fence points first") + 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]) - idx = int(args[0]) - if idx <= 0 or idx > self.fenceloader.count(): - print("Invalid fence point number %u" % idx) + 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 @@ -147,196 +340,299 @@ def cmd_fence_move(self, args): print("No map click position available") return - # note we don't subtract 1, as first fence point is the return point - self.fenceloader.move(idx, latlon[0], latlon[1]) - if self.send_fence(): - print("Moved fence point %u" % idx) + 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 - def cmd_fence_remove(self, args): - '''handle fencepoint remove''' - if len(args) < 1: - print("Usage: fence remove FENCEPOINTNUM") + 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 - if not self.have_list: - print("Please list fence points first") + + 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 - idx = int(args[0]) - if idx <= 0 or idx > self.fenceloader.count(): - print("Invalid fence point number %u" % idx) + 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() - # note we don't subtract 1, as first fence point is the return point - self.fenceloader.remove(idx) - if self.send_fence(): - print("Removed fence point %u" % idx) - else: - print("Failed to remove fence point %u" % idx) + 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 cmd_fence(self, args): - '''fence commands''' - if len(args) < 1: - self.print_usage() + 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 - if args[0] == "enable": - self.set_fence_enabled(1) - elif args[0] == "disable": - self.set_fence_enabled(0) - elif args[0] == "load": - if len(args) != 2: - print("usage: fence load ") - return - self.load_fence(args[1]) - elif args[0] == "list": - self.list_fence(None) - elif args[0] == "move": - self.cmd_fence_move(args[1:]) - elif args[0] == "remove": - self.cmd_fence_remove(args[1:]) - elif args[0] == "save": - if len(args) != 2: - print("usage: fence save ") - return - self.list_fence(args[1]) - elif args[0] == "show": - if len(args) != 2: - print("usage: fence show ") + 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 - self.fenceloader.load(args[1]) - self.have_list = True - elif args[0] == "draw": - if not 'draw_lines' in self.mpstate.map_functions: - print("No map drawing available") + 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 - self.mpstate.map_functions['draw_lines'](self.fence_draw_callback) - print("Drawing fence on map") - elif args[0] == "clear": - self.param_set('FENCE_TOTAL', 0, 3) + 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: - self.print_usage() - - def load_fence(self, filename): - '''load fence points from a file''' - try: - self.fenceloader.target_system = self.target_system - self.fenceloader.target_component = self.target_component - self.fenceloader.load(filename.strip('"')) - except Exception as msg: - print("Unable to load %s - %s" % (filename, msg)) - return - print("Loaded %u geo-fence points from %s" % (self.fenceloader.count(), filename)) - self.send_fence() - - def send_fence(self): - '''send fence points from fenceloader''' - # must disable geo-fencing when loading - self.fenceloader.target_system = self.target_system - self.fenceloader.target_component = self.target_component - self.fenceloader.reindex() - action = self.get_mav_param('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE) - self.param_set('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE, 3) - self.param_set('FENCE_TOTAL', self.fenceloader.count(), 3) - for i in range(self.fenceloader.count()): - p = self.fenceloader.point(i) - self.master.mav.send(p) - p2 = self.fetch_fence_point(i) - if p2 is None: - self.param_set('FENCE_ACTION', action, 3) - return False - if (p.idx != p2.idx or - abs(p.lat - p2.lat) >= 0.00003 or - abs(p.lng - p2.lng) >= 0.00003): - print("Failed to send fence point %u" % i) - self.param_set('FENCE_ACTION', action, 3) - return False - self.param_set('FENCE_ACTION', action, 3) - return True - - def fetch_fence_point(self ,i): - '''fetch one fence point''' - self.master.mav.fence_fetch_point_send(self.target_system, - self.target_component, i) - tstart = time.time() - p = None - while time.time() - tstart < 3: - p = self.master.recv_match(type='FENCE_POINT', blocking=False) - if p is not None: - break - time.sleep(0.1) - continue - if p is None: - self.console.error("Failed to fetch point %u" % i) - return None - return p + 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 fence_draw_callback(self, points): - '''callback from drawing a fence''' - self.fenceloader.clear() - if len(points) < 3: + 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 - self.fenceloader.target_system = self.target_system - self.fenceloader.target_component = self.target_component - bounds = mp_util.polygon_bounds(points) - (lat, lon, width, height) = bounds - center = (lat+width/2, lon+height/2) - self.fenceloader.add_latlon(center[0], center[1]) - for p in points: - self.fenceloader.add_latlon(p[0], p[1]) - # close it - self.fenceloader.add_latlon(points[0][0], points[0][1]) - self.send_fence() - self.have_list = True - - def list_fence(self, filename): - '''list fence points, optionally saving to a file''' - self.fenceloader.clear() - count = self.get_mav_param('FENCE_TOTAL', 0) - if count == 0: - print("No geo-fence points") - return - for i in range(int(count)): - for t in range(6): - p = self.fetch_fence_point(i) - if p is None: - print("retrying %u" % i) - continue - break - self.fenceloader.add(p) - - if filename is not None: - try: - self.fenceloader.save(filename.strip('"')) - except Exception as msg: - print("Unable to save %s - %s" % (filename, msg)) + + 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 - print("Saved %u geo-fence points to %s" % (self.fenceloader.count(), filename)) - else: - for i in range(self.fenceloader.count()): - p = self.fenceloader.point(i) - self.console.writeln("lat=%f lng=%f" % (p.lat, p.lng)) - if self.status.logdir is not None: - fname = 'fence.txt' - if self.target_system > 1: - fname = 'fence_%u.txt' % self.target_system - fencetxt = os.path.join(self.status.logdir, fname) - self.fenceloader.save(fencetxt.strip('"')) - print("Saved fence to %s" % fencetxt) - self.have_list = True - - def print_usage(self): - print("usage: fence ") - - def unload(self): - self.remove_command("fence") - 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(FenceModule, self).unload() + 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 + moving_item.x = int(moving_item.x) + moving_item.y = int(moving_item.y) + + 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''' + # see if pymavlink is new enough to support new protocols: + oldmodule = "fenceitem_protocol" + try: + mavwp.MissionItemProtocol_Fence + except AttributeError: + print("pymavlink too old; using old %s module" % oldmodule) + mpstate.load_module(oldmodule) + for (m, pm) in mpstate.modules: + if m.name == "fence": + return m + return None + return FenceModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_fenceitem_protocol.py b/MAVProxy/modules/mavproxy_fenceitem_protocol.py new file mode 100644 index 0000000000..4f7516143e --- /dev/null +++ b/MAVProxy/modules/mavproxy_fenceitem_protocol.py @@ -0,0 +1,342 @@ +""" + MAVProxy geofence module +""" +import os, time, platform +from pymavlink import mavwp, mavutil +from MAVProxy.modules.lib import mp_util +from MAVProxy.modules.lib import mp_module +if mp_util.has_wxpython: + from MAVProxy.modules.lib.mp_menu import * + +class FenceModule(mp_module.MPModule): + def __init__(self, mpstate): + super(FenceModule, self).__init__(mpstate, "fence", "geo-fence management", public = True) + self.fenceloader_by_sysid = {} + self.last_fence_breach = 0 + self.last_fence_status = 0 + self.sysid = 0 + self.compid = 0 + self.present = False + self.enabled = False + self.healthy = True + self.add_command('fence', self.cmd_fence, + "fence item protocol geo-fence management", + ["", + " (FILENAME)"]) + + self.have_list = False + + if self.continue_mode and self.logdir is not None: + fencetxt = os.path.join(self.logdir, 'fence.txt') + if os.path.exists(fencetxt): + self.fenceloader.load(fencetxt) + self.have_list = True + print("Loaded fence from %s" % fencetxt) + + self.menu_added_console = False + self.menu_added_map = False + if mp_util.has_wxpython: + self.menu = MPMenuSubMenu('Fence', + items=[MPMenuItem('Clear', 'Clear', '# fence clear'), + MPMenuItem('List', 'List', '# fence list'), + MPMenuItem('Load', 'Load', '# fence load ', + handler=MPMenuCallFileDialog(flags=('open',), + title='Fence Load', + wildcard='FenceFiles(*.txt,*.fen)|*.txt;*.fen')), + MPMenuItem('Save', 'Save', '# fence save ', + handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'), + title='Fence Save', + wildcard='FenceFiles(*.txt,*.fen)|*.txt;*.fen')), + MPMenuItem('Draw', 'Draw', '# fence draw')]) + + @property + def fenceloader(self): + '''fence loader by sysid''' + if not self.target_system in self.fenceloader_by_sysid: + self.fenceloader_by_sysid[self.target_system] = mavwp.MAVFenceLoader() + return self.fenceloader_by_sysid[self.target_system] + + def idle_task(self): + '''called on idle''' + 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 mavlink_packet(self, m): + '''handle and incoming mavlink packet''' + if self.sysid != 0 and (self.sysid != m.get_srcSystem() or self.compid != m.get_srcComponent()): + # ignore messages from other components we haven't locked on to + return + if m.get_type() == "FENCE_STATUS": + self.last_fence_breach = m.breach_time + self.last_fence_status = m.breach_status + self.compid = m.get_srcComponent() + self.sysid = m.get_srcSystem() + elif m.get_type() in ['SYS_STATUS']: + bits = mavutil.mavlink.MAV_SYS_STATUS_GEOFENCE + + present = ((m.onboard_control_sensors_present & bits) == bits) + if self.present == False and present == True: + self.say("fence present") + self.compid = m.get_srcComponent() + self.sysid = m.get_srcSystem() + elif self.present == True and present == False: + self.say("fence removed") + self.present = present + + enabled = ((m.onboard_control_sensors_enabled & bits) == bits) + if self.enabled == False and enabled == True: + self.say("fence enabled") + self.compid = m.get_srcComponent() + self.sysid = m.get_srcSystem() + elif self.enabled == True and enabled == False: + self.say("fence disabled") + self.enabled = enabled + + healthy = ((m.onboard_control_sensors_health & bits) == bits) + if self.healthy == False and healthy == True: + self.say("fence OK") + elif self.healthy == True and healthy == False: + self.say("fence breach") + self.healthy = healthy + + #console output for fence: + if not self.present: + self.console.set_status('Fence', 'FEN', row=0, fg='black') + elif self.enabled == False: + self.console.set_status('Fence', 'FEN', row=0, fg='grey') + elif self.enabled == True and self.healthy == True: + self.console.set_status('Fence', 'FEN', row=0, fg='green') + elif self.enabled == True and self.healthy == False: + self.console.set_status('Fence', 'FEN', row=0, fg='red') + + 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_fence_move(self, args): + '''handle fencepoint move''' + if len(args) < 1: + print("Usage: fence move FENCEPOINTNUM") + return + if not self.have_list: + print("Please list fence points first") + return + + idx = int(args[0]) + if idx <= 0 or idx > self.fenceloader.count(): + print("Invalid fence point number %u" % idx) + return + + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + + # note we don't subtract 1, as first fence point is the return point + self.fenceloader.move(idx, latlon[0], latlon[1]) + if self.send_fence(): + print("Moved fence point %u" % idx) + + def cmd_fence_remove(self, args): + '''handle fencepoint remove''' + if len(args) < 1: + print("Usage: fence remove FENCEPOINTNUM") + return + if not self.have_list: + print("Please list fence points first") + return + + idx = int(args[0]) + if idx <= 0 or idx > self.fenceloader.count(): + print("Invalid fence point number %u" % idx) + return + + # note we don't subtract 1, as first fence point is the return point + self.fenceloader.remove(idx) + if self.send_fence(): + print("Removed fence point %u" % idx) + else: + print("Failed to remove fence point %u" % idx) + + def cmd_fence(self, args): + '''fence commands''' + if len(args) < 1: + self.print_usage() + return + + if args[0] == "enable": + self.set_fence_enabled(1) + elif args[0] == "disable": + self.set_fence_enabled(0) + elif args[0] == "load": + if len(args) != 2: + print("usage: fence load ") + return + self.load_fence(args[1]) + elif args[0] == "list": + self.list_fence(None) + elif args[0] == "move": + self.cmd_fence_move(args[1:]) + elif args[0] == "remove": + self.cmd_fence_remove(args[1:]) + elif args[0] == "save": + if len(args) != 2: + print("usage: fence save ") + return + self.list_fence(args[1]) + elif args[0] == "show": + if len(args) != 2: + print("usage: fence show ") + return + self.fenceloader.load(args[1]) + self.have_list = True + elif args[0] == "draw": + if not 'draw_lines' in self.mpstate.map_functions: + print("No map drawing available") + return + self.mpstate.map_functions['draw_lines'](self.fence_draw_callback) + print("Drawing fence on map") + elif args[0] == "clear": + self.param_set('FENCE_TOTAL', 0, 3) + else: + self.print_usage() + + def load_fence(self, filename): + '''load fence points from a file''' + try: + self.fenceloader.target_system = self.target_system + self.fenceloader.target_component = self.target_component + self.fenceloader.load(filename.strip('"')) + except Exception as msg: + print("Unable to load %s - %s" % (filename, msg)) + return + print("Loaded %u geo-fence points from %s" % (self.fenceloader.count(), filename)) + self.send_fence() + + def send_fence(self): + '''send fence points from fenceloader''' + # must disable geo-fencing when loading + self.fenceloader.target_system = self.target_system + self.fenceloader.target_component = self.target_component + self.fenceloader.reindex() + action = self.get_mav_param('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE) + self.param_set('FENCE_ACTION', mavutil.mavlink.FENCE_ACTION_NONE, 3) + self.param_set('FENCE_TOTAL', self.fenceloader.count(), 3) + for i in range(self.fenceloader.count()): + p = self.fenceloader.point(i) + self.master.mav.send(p) + p2 = self.fetch_fence_point(i) + if p2 is None: + self.param_set('FENCE_ACTION', action, 3) + return False + if (p.idx != p2.idx or + abs(p.lat - p2.lat) >= 0.00003 or + abs(p.lng - p2.lng) >= 0.00003): + print("Failed to send fence point %u" % i) + self.param_set('FENCE_ACTION', action, 3) + return False + self.param_set('FENCE_ACTION', action, 3) + return True + + def fetch_fence_point(self ,i): + '''fetch one fence point''' + self.master.mav.fence_fetch_point_send(self.target_system, + self.target_component, i) + tstart = time.time() + p = None + while time.time() - tstart < 3: + p = self.master.recv_match(type='FENCE_POINT', blocking=False) + if p is not None: + break + time.sleep(0.1) + continue + if p is None: + self.console.error("Failed to fetch point %u" % i) + return None + return p + + def fence_draw_callback(self, points): + '''callback from drawing a fence''' + self.fenceloader.clear() + if len(points) < 3: + return + self.fenceloader.target_system = self.target_system + self.fenceloader.target_component = self.target_component + bounds = mp_util.polygon_bounds(points) + (lat, lon, width, height) = bounds + center = (lat+width/2, lon+height/2) + self.fenceloader.add_latlon(center[0], center[1]) + for p in points: + self.fenceloader.add_latlon(p[0], p[1]) + # close it + self.fenceloader.add_latlon(points[0][0], points[0][1]) + self.send_fence() + self.have_list = True + + def list_fence(self, filename): + '''list fence points, optionally saving to a file''' + self.fenceloader.clear() + count = self.get_mav_param('FENCE_TOTAL', 0) + if count == 0: + print("No geo-fence points") + return + for i in range(int(count)): + for t in range(6): + p = self.fetch_fence_point(i) + if p is None: + print("retrying %u" % i) + continue + break + self.fenceloader.add(p) + + if filename is not None: + try: + self.fenceloader.save(filename.strip('"')) + except Exception as msg: + print("Unable to save %s - %s" % (filename, msg)) + return + print("Saved %u geo-fence points to %s" % (self.fenceloader.count(), filename)) + else: + for i in range(self.fenceloader.count()): + p = self.fenceloader.point(i) + self.console.writeln("lat=%f lng=%f" % (p.lat, p.lng)) + if self.status.logdir is not None: + fname = 'fence.txt' + if self.target_system > 1: + fname = 'fence_%u.txt' % self.target_system + fencetxt = os.path.join(self.status.logdir, fname) + self.fenceloader.save(fencetxt.strip('"')) + print("Saved fence to %s" % fencetxt) + self.have_list = True + + def print_usage(self): + print("usage: fence ") + + def unload(self): + self.remove_command("fence") + 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(FenceModule, self).unload() + +def init(mpstate): + '''initialise module''' + return FenceModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 4e1936775c..5dc980457c 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -32,6 +32,7 @@ def __init__(self, mpstate): self.moving_fencepoint = None self.moving_rally = None self.mission_list = None + self.moving_polygon_point = None self.icon_counter = 0 self.circle_counter = 0 self.draw_line = None @@ -259,6 +260,7 @@ def display_waypoints(self): items = [MPMenuItem('WP Set', returnkey='popupMissionSet'), MPMenuItem('WP Remove', returnkey='popupMissionRemove'), MPMenuItem('WP Move', returnkey='popupMissionMove'), + MPMenuItem('WP Split', returnkey='popupMissionSplit'), ] popup = MPMenuSubMenu('Popup', items) self.map.add_object(mp_slipmap.SlipPolygon('mission %u' % i, p, @@ -296,10 +298,132 @@ def display_waypoints(self): labeled_wps[next_list[j]] = (i,j) + # Start: handling of PolyFence popup menu items + def polyfence_remove_circle(self, id): + '''called when a fence is right-clicked and remove is selected; + removes the circle + ''' + (seq, type) = id.split(":") + self.module('fence').removecircle(int(seq)) + + def polyfence_remove_polygon(self, id): + '''called when a fence is right-clicked and remove is selected; + removes the polygon + ''' + (seq, type) = id.split(":") + self.module('fence').removepolygon(int(seq)) + + def polyfence_remove_polygon_point(self, id, extra): + '''called when a fence is right-clicked and remove point is selected; + removes the polygon point + ''' + (seq, type) = id.split(":") + self.module('fence').removepolygon_point(int(seq), int(extra)) + + def polyfence_add_polygon_point(self, id, extra): + '''called when a fence is right-clicked and add point is selected; + adds a polygon *before* this one in the list + ''' + (seq, type) = id.split(":") + self.module('fence').addpolygon_point(int(seq), int(extra)) + + def polyfence_move_polygon_point(self, id, extra): + '''called when a fence is right-clicked and move point is selected; start + moving the polygon point + ''' + (seq, t) = id.split(":") + self.moving_polygon_point = (int(seq), extra) + # End: handling of PolyFence popup menu items + + def display_polyfences_circles(self, circles, colour): + '''draws circles in the PolyFence layer with colour colour''' + for circle in circles: + lat = circle.x + lng = circle.y + if circle.get_type() == 'MISSION_ITEM_INT': + lat *= 1e-7 + lng *= 1e-7 + items = [ + MPMenuItem('Remove Circle', returnkey='popupPolyFenceRemoveCircle'), + ] + popup = MPMenuSubMenu('Popup', items) + slipcircle = mp_slipmap.SlipCircle( + str(circle.seq) + ":circle", # key + "PolyFence", # layer + (lat, lng), # latlon + circle.param1, # radius + colour, + linewidth=2, + popup_menu=popup) + self.map.add_object(slipcircle) + + def display_polyfences_inclusion_circles(self): + '''draws inclusion circles in the PolyFence layer with colour colour''' + inclusions = self.module('fence').inclusion_circles() + self.display_polyfences_circles(inclusions, (0, 255, 0)) + + def display_polyfences_exclusion_circles(self): + '''draws exclusion circles in the PolyFence layer with colour colour''' + exclusions = self.module('fence').exclusion_circles() + self.display_polyfences_circles(exclusions, (255, 0, 0)) + + def display_polyfences_polygons(self, polygons, colour): + '''draws polygons in the PolyFence layer with colour colour''' + for polygon in polygons: + points = [] + for point in polygon: + lat = point.x + lng = point.y + if point.get_type() == 'MISSION_ITEM_INT': + lat *= 1e-7 + lng *= 1e-7 + points.append((lat, lng)) + items = [ + MPMenuItem('Remove Polygon', returnkey='popupPolyFenceRemovePolygon'), + ] + if len(points) > 3: + items.append(MPMenuItem('Remove Polygon Point', returnkey='popupPolyFenceRemovePolygonPoint')) + items.append(MPMenuItem('Move Polygon Point', returnkey='popupPolyFenceMovePolygonPoint')) + items.append(MPMenuItem('Add Polygon Point', returnkey='popupPolyFenceAddPolygonPoint')) + + popup = MPMenuSubMenu('Popup', items) + poly = mp_slipmap.UnclosedSlipPolygon( + str(polygon[0].seq) + ":poly", # key, + points, + layer='PolyFence', + linewidth=2, + colour=colour, + popup_menu=popup) + self.map.add_object(poly) + + def display_polyfences_inclusion_polygons(self): + '''draws inclusion polygons in the PolyFence layer with colour colour''' + inclusions = self.module('fence').inclusion_polygons() + self.display_polyfences_polygons(inclusions, (0, 255, 0)) + + def display_polyfences_exclusion_polygons(self): + '''draws exclusion polygons in the PolyFence layer with colour colour''' + exclusions = self.module('fence').exclusion_polygons() + self.display_polyfences_polygons(exclusions, (255, 0, 0)) + + def display_polyfences(self): + '''draws PolyFence items in the PolyFence layer''' + self.map.add_object(mp_slipmap.SlipClearLayer('PolyFence')) + self.display_polyfences_inclusion_circles() + self.display_polyfences_exclusion_circles() + self.display_polyfences_inclusion_polygons() + self.display_polyfences_exclusion_polygons() + def display_fence(self): '''display the fence''' from MAVProxy.modules.mavproxy_map import mp_slipmap - self.fence_change_time = self.module('fence').fenceloader.last_change + if getattr(self.module('fence'), "cmd_addcircle", None) is not None: + # we're doing fences via MissionItemProtocol and thus have + # much more work to do + return self.display_polyfences() + + # traditional module, a single polygon fence transfered using + # FENCE_POINT protocol: points = self.module('fence').fenceloader.polygon() self.map.add_object(mp_slipmap.SlipClearLayer('Fence')) if len(points) > 1: @@ -374,6 +498,11 @@ def remove_mission(self, key, selection_index): idx = self.selection_index_to_idx(key, selection_index) self.mpstate.functions.process_stdin('wp remove %u' % idx) + def split_mission_wp(self, key, selection_index): + '''add wp before this one''' + idx = self.selection_index_to_idx(key, selection_index) + self.mpstate.functions.process_stdin('wp split %u' % idx) + def remove_fencepoint(self, key, selection_index): '''remove a fence point''' self.mpstate.functions.process_stdin('fence remove %u' % (selection_index+1)) @@ -408,10 +537,20 @@ def handle_menu_event(self, obj): self.remove_mission(obj.selected[0].objkey, obj.selected[0].extra_info) elif menuitem.returnkey == 'popupMissionMove': self.move_mission(obj.selected[0].objkey, obj.selected[0].extra_info) - elif menuitem.returnkey == 'popupFenceRemove': - self.remove_fencepoint(obj.selected[0].objkey, obj.selected[0].extra_info) + elif menuitem.returnkey == 'popupMissionSplit': + self.split_mission_wp(obj.selected[0].objkey, obj.selected[0].extra_info) elif menuitem.returnkey == 'popupFenceMove': self.move_fencepoint(obj.selected[0].objkey, obj.selected[0].extra_info) + elif menuitem.returnkey == 'popupPolyFenceRemoveCircle': + self.polyfence_remove_circle(obj.selected[0].objkey) + elif menuitem.returnkey == 'popupPolyFenceRemovePolygon': + self.polyfence_remove_polygon(obj.selected[0].objkey) + elif menuitem.returnkey == 'popupPolyFenceMovePolygonPoint': + self.polyfence_move_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info) + elif menuitem.returnkey == 'popupPolyFenceAddPolygonPoint': + self.polyfence_add_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info) + elif menuitem.returnkey == 'popupPolyFenceRemovePolygonPoint': + self.polyfence_remove_polygon_point(obj.selected[0].objkey, obj.selected[0].extra_info) elif menuitem.returnkey == 'showPosition': self.show_position() elif menuitem.returnkey == 'printGoogleMapsLink': @@ -450,6 +589,16 @@ def map_callback(self, obj): print("Cancelled wp move") self.moving_wp = None return + if obj.event.leftIsDown and self.moving_polygon_point is not None: + self.mpstate.click(obj.latlon) + (seq, offset) = self.moving_polygon_point + self.mpstate.functions.process_stdin("fence movepolypoint %u %u" % (int(seq), int(offset))) + self.moving_polygon_point = None + return + if obj.event.rightIsDown and self.moving_polygon_point is not None: + print("Cancelled polygon point move") + self.moving_polygon_point = None + return if obj.event.rightIsDown and self.moving_fencepoint is not None: print("Cancelled fence move") self.moving_fencepoint = None @@ -509,7 +658,7 @@ def drawing_update(self): self.draw_line.append(self.mpstate.click_location) if len(self.draw_line) > 1: self.map.add_object(mp_slipmap.SlipPolygon('drawing', self.draw_line, - layer='Drawing', linewidth=2, colour=(128,128,255))) + layer='Drawing', linewidth=2, colour=self.draw_colour)) def drawing_end(self): '''end line drawing''' @@ -521,10 +670,11 @@ def drawing_end(self): self.map.add_object(mp_slipmap.SlipDefaultPopup(self.default_popup, combine=True)) self.map.add_object(mp_slipmap.SlipClearLayer('Drawing')) - def draw_lines(self, callback): + def draw_lines(self, callback, colour=(128,128,255)): '''draw a series of connected lines on the map, calling callback when done''' from MAVProxy.modules.mavproxy_map import mp_slipmap self.draw_callback = callback + self.draw_colour = colour self.draw_line = [] self.map.add_object(mp_slipmap.SlipDefaultPopup(None)) @@ -824,9 +974,17 @@ def mavlink_packet(self, m): self.rally_change_time = time.time() # if the fence has changed, redisplay - if (self.module('fence') and - self.fence_change_time != self.module('fence').fenceloader.last_change): - self.display_fence() + fence_module = self.module('fence') + if fence_module is not None: + if hasattr(fence_module, 'last_change'): + # new fence module + last_change = fence_module.last_change() + else: + # old fence module + last_change = fence_module.fenceloader.last_change + if self.fence_change_time != last_change: + self.fence_change_time = last_change + self.display_fence() # if the rallypoints have changed, redisplay if (self.module('rally') and diff --git a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py index 521a07c6d2..a3e949a01a 100644 --- a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py +++ b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py @@ -185,6 +185,9 @@ def draw(self, img, pixmapper, bounds): self.color, self.linewidth, 0, reverse = self.reverse).draw(img) SlipArrow(self.key, self.layer, (center_px[0]+radius_px, center_px[1]), self.color, self.linewidth, math.pi, reverse = self.reverse).draw(img) + # stash some values for determining closest click location + self.radius_px = radius_px + self.center_px = center_px def bounds(self): '''return bounding box''' @@ -192,6 +195,22 @@ def bounds(self): return None return (self.latlon[0], self.latlon[1], 0, 0) + def clicked(self, px, py): + '''check if a click on px,py should be considered a click + on the object. Return None if definitely not a click, + otherwise return the distance of the click, smaller being nearer + ''' + radius_px = getattr(self, "radius_px", None) + if radius_px is None: + return + + dx = self.center_px[0] - px + dy = self.center_px[1] - py + ret = abs(math.sqrt(dx*dx+dy*dy) - radius_px) + if ret > 10: # threshold of within 10 pixels for a click to count + return None + return ret + class SlipPolygon(SlipObject): '''a polygon to display on the map''' def __init__(self, key, points, layer, colour, linewidth, arrow = False, popup_menu=None, showlines=True): @@ -276,6 +295,31 @@ def selection_info(self): '''extra selection information sent when object is selected''' return self._selected_vertex +class UnclosedSlipPolygon(SlipPolygon): + '''a polygon to display on the map - but one with no return point or + closing vertex''' + def draw(self, img, pixmapper, bounds, colour=(0,0,0)): + '''draw a polygon on the image''' + if self.hidden: + return + self._pix_points = [] + for i in range(len(self.points)): + if len(self.points[i]) > 2: + colour = self.points[i][2] + else: + colour = self.colour + _from = self.points[i] + if i+1 == len(self.points): + _to = self.points[0] + else: + _to = self.points[i+1] + self.draw_line( + img, + pixmapper, + _from, + _to, + colour, + self.linewidth) class SlipGrid(SlipObject): '''a map grid''' 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_oldwp.py b/MAVProxy/modules/mavproxy_oldwp.py new file mode 100644 index 0000000000..d1bd911074 --- /dev/null +++ b/MAVProxy/modules/mavproxy_oldwp.py @@ -0,0 +1,1329 @@ +#!/usr/bin/env python +'''waypoint command handling''' + +import copy +import os +import struct +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): + 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) + + 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'), + ] + ) + + @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 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 is_quadplane(self): + Q_ENABLE = int(self.get_mav_param("Q_ENABLE", 0)) + return Q_ENABLE > 0 + + 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 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 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 m.seq != self.last_waypoint: + self.last_waypoint = m.seq + if self.settings.wpupdates: + self.say("waypoint %u" % m.seq, priority='message') + + elif mtype == "MISSION_ITEM_REACHED": + wp = self.wploader.wp(m.seq) + if wp is None: + # should we spit out a warning?! + # self.say("No waypoints") + pass + else: + if wp.command == mavutil.mavlink.MAV_CMD_DO_LAND_START: + alt_offset = self.get_mav_param('ALT_OFFSET', 0) + if alt_offset > 0.005: + self.say("ALT OFFSET IS NOT ZERO passing DO_LAND_START") + + elif mtype == "COMMAND_ACK": + # check to see if the vehicle has bounced our attempts to + # set the current mission item via mavlink command (as + # opposed to the old message): + if m.command == mavutil.mavlink.MAV_CMD_DO_SET_MISSION_CURRENT: + key = (m.get_srcSystem(), m.get_srcComponent()) + if m.result == mavutil.mavlink.MAV_RESULT_UNSUPPORTED: + # stop sending the commands: + self.accepts_DO_SET_MISSION_CURRENT[key] = False + 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 + + 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 + if (self.master is not None and + 'HOME_POSITION' not in self.master.messages and + time.time() - self.last_get_home > 2): + self.master.mav.command_long_send( + self.settings.target_system, + 0, + mavutil.mavlink.MAV_CMD_GET_HOME_POSITION, + 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 + + 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): + '''handle wp move to a point relative to home by dist/bearing''' + if len(args) < 3: + print("usage: wp moverelhome WPNUM dist bearing") + return + idx = int(args[0]) + if idx < 1 or idx > self.wploader.count(): + print("Invalid wp number %u" % idx) + return + dist = float(args[1]) + bearing = float(args[2]) + + home = self.get_home() + if home is None: + print("Need home") + return + + wp = self.wploader.wp(idx) + if not self.is_location_wp(wp): + print("Not a nav command") + return + (newlat, newlon) = mp_util.gps_newpos(home.x, home.y, bearing, dist) + wp.x = newlat + wp.y = newlon + wp.target_system = self.target_system + wp.target_component = self.target_component + self.wploader.set(wp, 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+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 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(" wp%u %s" % (wp1, wp2, slope)) + return + if len(args) != 0: + print("Usage: wp slope WP1 WP2") + return + last_w = None + for i in range(1, self.wploader.count()): + w = self.wploader.wp(i) + if w.command not in [mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, mavutil.mavlink.MAV_CMD_NAV_LAND]: + continue + if last_w is not None: + if last_w.frame != w.frame: + print("WARNING: frame change %u -> %u at %u" % (last_w.frame, w.frame, i)) + delta_alt = last_w.z - w.z + if delta_alt == 0: + slope = "Level" + else: + delta_xy = mp_util.gps_distance(w.x, w.y, last_w.x, last_w.y) + slope = "%.1f" % (delta_xy / delta_alt) + print("WP%u: slope %s" % (i, slope)) + last_w = w + + def get_default_frame(self): + '''default frame for waypoints''' + if self.settings.terrainalt == 'Auto': + if self.get_mav_param('TERRAIN_FOLLOW', 0) == 1: + return mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT + return mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + if self.settings.terrainalt == 'True': + return mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT + return mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + + def get_home(self): + '''get home location''' + if 'HOME_POSITION' in self.master.messages: + h = self.master.messages['HOME_POSITION'] + return mavutil.mavlink.MAVLink_mission_item_message(self.target_system, + self.target_component, + 0, + 0, + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, + 0, 0, 0, 0, 0, 0, + h.latitude*1.0e-7, h.longitude*1.0e-7, h.altitude*1.0e-3) + if self.wploader.count() > 0: + return self.wploader.wp(0) + return None + + def wp_draw_callback(self, points): + '''callback from drawing waypoints''' + if len(points) < 3: + return + self.wploader.target_system = self.target_system + self.wploader.target_component = self.target_component + if self.wploader.count() < 2: + home = self.get_home() + if home is None: + print("Need home location for draw - please run gethome") + return + self.wploader.clear() + self.wploader.add(home) + if self.get_default_frame() == mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT: + use_terrain = True + else: + use_terrain = False + for p in points: + self.wploader.add_latlonalt(p[0], p[1], self.settings.wpalt, terrain_alt=use_terrain) + self.send_all_waypoints() + + def wp_loop(self): + '''close the loop on a mission''' + loader = self.wploader + if loader.count() < 2: + print("Not enough waypoints (%u)" % loader.count()) + return + wp = loader.wp(loader.count()-2) + if wp.command == mavutil.mavlink.MAV_CMD_DO_JUMP: + print("Mission is already looped") + return + if (loader.count() > 1 and + loader.wp(1).command in [mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF]): + target = 2 + else: + target = 1 + wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, 0, mavutil.mavlink.MAV_CMD_DO_JUMP, + 0, 1, target, -1, 0, 0, 0, 0, 0) + loader.add(wp) + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.waypoint_count_send(self.wploader.count()) + print("Closed loop on mission") + + def wp_add_takeoff(self, args): + '''add a takeoff as first mission item''' + latlon = self.mpstate.click_location + if latlon is None: + print("No position chosen") + return + takeoff_alt = 20 + if len(args) > 0: + takeoff_alt = float(args[0]) + if self.is_quadplane(): + wptype = mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF + else: + wptype = mavutil.mavlink.MAV_CMD_NAV_TAKEOFF + wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + wptype, + 0, 1, 0, 0, 0, 0, latlon[0], latlon[1], takeoff_alt) + if self.wploader.count() < 2: + home = self.get_home() + if home is None: + print("Need home location - please run gethome") + return + self.wploader.clear() + self.wploader.add(home) + # assume first waypoint + self.wploader.insert(1, wp) + self.send_all_waypoints() + + def wp_add_landing(self): + '''add a landing as last mission item''' + latlon = self.mpstate.click_location + if latlon is None: + print("No position chosen") + return + if self.is_quadplane(): + wptype = mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND + else: + wptype = mavutil.mavlink.MAV_CMD_NAV_LAND + wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + wptype, + 0, 1, 0, 0, 0, 0, latlon[0], latlon[1], 0) + # assume last waypoint + self.wploader.add(wp) + self.send_all_waypoints() + + def wp_add_RTL(self): + '''add a RTL as last mission item''' + 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, + 0, 1, 0, 0, 0, 0, 0, 0, 0) + # assume last waypoint + self.wploader.add(wp) + self.send_all_waypoints() + + def wp_add_dls(self): + '''add a DO_LAND_START as last mission item''' + latlon = self.mpstate.click_location + if latlon is None: + print("No position chosen") + return + wp = mavutil.mavlink.MAVLink_mission_item_message(0, 0, 0, + mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, + mavutil.mavlink.MAV_CMD_DO_LAND_START, + 0, 1, 0, 0, 0, 0, latlon[0], latlon[1], 0) + # assume last waypoint + self.wploader.add(wp) + self.send_all_waypoints() + + def set_home_location(self): + '''set home location from last map click''' + latlon = self.mpstate.click_location + if latlon is None: + print("No position available") + return + lat = float(latlon[0]) + lon = float(latlon[1]) + if self.wploader.count() == 0: + self.wploader.add_latlonalt(lat, lon, 0) + w = self.wploader.wp(0) + w.x = lat + w.y = lon + self.wploader.set(w, 0) + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + 0, 0 + ) + + def fix_jumps(self, idx, delta): + '''fix up jumps when we add/remove rows''' + numrows = self.wploader.count() + for row in range(numrows): + wp = self.wploader.wp(row) + jump_cmds = [mavutil.mavlink.MAV_CMD_DO_JUMP] + if hasattr(mavutil.mavlink, "MAV_CMD_DO_CONDITION_JUMP"): + jump_cmds.append(mavutil.mavlink.MAV_CMD_DO_CONDITION_JUMP) + if wp.command in jump_cmds: + p1 = int(wp.param1) + if p1 > idx and p1 + delta > 0: + wp.param1 = float(p1+delta) + self.wploader.set(wp, row) + + def get_loc(self, m): + '''return a mavutil.location for item m''' + t = m.get_type() + if t == "MISSION_ITEM": + lat = m.x * 1e7 + lng = m.y * 1e7 + alt = m.z * 1e2 + elif t == "MISSION_ITEM_INT": + lat = m.x + lng = m.y + alt = m.z + else: + return None + return mavutil.location(lat, lng, alt) + + def cmd_split(self, args): + '''splits the segment ended by the supplied waypoint into two''' + try: + num = int(args[0]) + except IOError: + return "Bad wp num (%s)" % args[0] + + if num < 1 or num > self.wploader.count(): + print("Bad item %s" % str(num)) + return + wp = self.wploader.wp(num) + if wp is None: + print("Could not get wp %u" % num) + return + loc = self.get_loc(wp) + if loc is None: + print("wp is not a location command") + return + + prev = num - 1 + if prev < 1 or prev > self.wploader.count(): + print("Bad item %u" % num) + return + prev_wp = self.wploader.wp(prev) + if prev_wp is None: + print("Could not get previous wp %u" % prev) + return + prev_loc = self.get_loc(prev_wp) + if prev_loc is None: + print("previous wp is not a location command") + return + + if wp.frame != prev_wp.frame: + print("waypoints differ in frame (%u vs %u)" % + (wp.frame, prev_wp.frame)) + return + + if wp.frame != prev_wp.frame: + print("waypoints differ in frame") + return + + lat_avg = (loc.lat + prev_loc.lat)/2 + lng_avg = (loc.lng + prev_loc.lng)/2 + alt_avg = (loc.alt + prev_loc.alt)/2 + new_wp = mavutil.mavlink.MAVLink_mission_item_message( + self.target_system, + self.target_component, + wp.seq, # seq + wp.frame, # frame + mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, # command + 0, # current + 0, # autocontinue + 0.0, # param1, + 0.0, # param2, + 0.0, # param3 + 0.0, # param4 + lat_avg * 1e-7, # x (latitude) + lng_avg * 1e-7, # y (longitude) + alt_avg * 1e-2, # z (altitude) + ) + self.wploader.insert(wp.seq, new_wp) + self.fix_jumps(wp.seq, 1) + self.send_all_waypoints() + + +def init(mpstate): + '''initialise module''' + return WPModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_rally.py b/MAVProxy/modules/mavproxy_rally.py index 2c20ee7875..f788750f72 100644 --- a/MAVProxy/modules/mavproxy_rally.py +++ b/MAVProxy/modules/mavproxy_rally.py @@ -1,360 +1,153 @@ -""" - MAVProxy rally module -""" +'''rally point handling via MissionItemProtocol''' + +from MAVProxy.modules.lib import mission_item_protocol -from pymavlink import mavwp from pymavlink import mavutil -import time, os, platform -from MAVProxy.modules.lib import mp_module +from pymavlink import mavwp + from MAVProxy.modules.lib import mp_util if mp_util.has_wxpython: - from MAVProxy.modules.lib.mp_menu import * - -class RallyModule(mp_module.MPModule): - def __init__(self, mpstate): - super(RallyModule, self).__init__(mpstate, "rally", "rally point control", public = True) - self.rallyloader_by_sysid = {} - self.add_command('rally', self.cmd_rally, "rally point control", ["", - " (FILENAME)"]) - self.have_list = False - self.abort_alt = 50 - self.abort_first_send_time = 0 - self.abort_previous_send_time = 0 - self.abort_ack_received = True - - self.menu_added_console = False - self.menu_added_map = False - if mp_util.has_wxpython: - self.menu = MPMenuSubMenu('Rally', - items=[MPMenuItem('Clear', 'Clear', '# rally clear'), - MPMenuItem('List', 'List', '# rally list'), - MPMenuItem('Load', 'Load', '# rally load ', - handler=MPMenuCallFileDialog(flags=('open',), - title='Rally Load', - wildcard='RallyPoints(*.txt,*.rally,*.ral)|*.txt;*.rally;*.ral')), - MPMenuItem('Save', 'Save', '# rally save ', - handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'), - title='Rally Save', - wildcard='RallyPoints(*.txt,*.rally,*.ral)|*.txt;*.rally;*.ral')), - MPMenuItem('Add', 'Add', '# rally add ', - handler=MPMenuCallTextDialog(title='Rally Altitude (m)', - default=100))]) - - @property - def rallyloader(self): - '''rally loader by system ID''' - if not self.target_system in self.rallyloader_by_sysid: - self.rallyloader_by_sysid[self.target_system] = mavwp.MAVRallyLoader(self.settings.target_system, - self.settings.target_component) - return self.rallyloader_by_sysid[self.target_system] - - def last_change(self): - '''return time of last changes made to rally points''' - return self.rallyloader.last_change - - def rally_count(self): - '''return number of waypoints''' - return self.rallyloader.rally_count() - - def rally_point(self, i): - '''return instance of mavutil.mavlink.MAVLink_rally_point_message''' - return self.rallyloader.rally_point(i) + from MAVProxy.modules.lib.mp_menu import MPMenuCallTextDialog + from MAVProxy.modules.lib.mp_menu import MPMenuItem - def set_last_change(self, time): - '''can be used to cause map redraws''' - self.rallyloader.last_change = time - def idle_task(self): - '''called on idle''' - 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 +class RallyModule(mission_item_protocol.MissionItemProtocolModule): - '''handle abort command; it is critical that the AP to receive it''' - if self.abort_ack_received is False: - #only send abort every second (be insistent, but don't spam) - if (time.time() - self.abort_previous_send_time > 1): - self.master.mav.command_long_send(self.settings.target_system, - self.settings.target_component, - mavutil.mavlink.MAV_CMD_DO_GO_AROUND, - 0, int(self.abort_alt), 0, 0, 0, 0, 0, 0,) - self.abort_previous_send_time = time.time() - - #try to get an ACK from the plane: - if self.abort_first_send_time == 0: - self.abort_first_send_time = time.time() - elif time.time() - self.abort_first_send_time > 10: #give up after 10 seconds - print("Unable to send abort command!\n") - self.abort_ack_received = True + def __init__(self, mpstate): + '''initialise module''' + 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): - '''handle rally add''' - if len(args) < 1: - alt = self.settings.rallyalt - else: - alt = float(args[0]) - - if len(args) < 2: - break_alt = self.settings.rally_breakalt - else: - break_alt = float(args[1]) - - if len(args) < 3: - flag = self.settings.rally_flags - else: - flag = int(args[2]) - #currently only supporting autoland values: - #True (nonzero) and False (zero) - if (flag != 0): - flag = 2 - - if not self.have_list: - print("Please list rally points first") + '''add a rally point at the last map click position''' + if not self.check_have_list(): return - - if (self.rallyloader.rally_count() > 4): - print("Only 5 rally points possible per flight plan.") - return - latlon = self.mpstate.click_location if latlon is None: - print("No map click position available") - return - - land_hdg = 0.0 - - self.rallyloader.create_and_append_rally_point(latlon[0] * 1e7, latlon[1] * 1e7, alt, break_alt, land_hdg, flag) - self.send_rally_points() - print("Added Rally point at %s %f %f, autoland: %s" % (str(latlon), alt, break_alt, bool(flag & 2))) - - def cmd_rally_alt(self, args): - '''handle rally alt change''' - if (len(args) < 2): - print("Usage: rally alt RALLYNUM newAlt ") - return - if not self.have_list: - print("Please list rally points first") + print("No click position available") return - idx = int(args[0]) - if idx <= 0 or idx > self.rallyloader.rally_count(): - print("Invalid rally point number %u" % idx) - return - - new_alt = int(args[1]) - new_break_alt = None - if (len(args) > 2): - new_break_alt = int(args[2]) - - self.rallyloader.set_alt(idx, new_alt, new_break_alt) - self.send_rally_point(idx-1) - self.fetch_rally_point(idx-1) - self.rallyloader.reindex() - - def cmd_rally_move(self, args): - '''handle rally move''' if len(args) < 1: - print("Usage: rally move RALLYNUM") - return - if not self.have_list: - print("Please list rally points first") - return - - idx = int(args[0]) - if idx <= 0 or idx > self.rallyloader.rally_count(): - print("Invalid rally point number %u" % idx) - return - - rpoint = self.rallyloader.rally_point(idx-1) - - latlon = self.mpstate.click_location - if latlon is None: - print("No map click position available") - return - - oldpos = (rpoint.lat*1e-7, rpoint.lng*1e-7) - self.rallyloader.move(idx, latlon[0], latlon[1]) - self.send_rally_point(idx-1) - p = self.fetch_rally_point(idx-1) - if p.lat != int(latlon[0]*1e7) or p.lng != int(latlon[1]*1e7): - print("Rally move failed") - return - self.rallyloader.reindex() - print("Moved rally point from %s to %s at %fm" % (str(oldpos), str(latlon), rpoint.alt)) - - - def cmd_rally(self, args): - '''rally point commands''' - #TODO: add_land arg - if len(args) < 1: - self.print_usage() - return - - elif args[0] == "add": - self.cmd_rally_add(args[1:]) - - elif args[0] == "move": - self.cmd_rally_move(args[1:]) - - elif args[0] == "clear": - self.rallyloader.clear() - self.mav_param.mavset(self.master,'RALLY_TOTAL',0,3) - - elif args[0] == "remove": - if not self.have_list: - print("Please list rally points first") - return - if (len(args) < 2): - print("Usage: rally remove RALLYNUM") - return - self.rallyloader.remove(int(args[1])) - self.send_rally_points() - - elif args[0] == "list": - self.list_rally_points() - self.have_list = True - - elif args[0] == "load": - if (len(args) < 2): - print("Usage: rally load filename") - return - - try: - self.rallyloader.load(args[1].strip('"')) - except Exception as msg: - print("Unable to load %s - %s" % (args[1], msg)) - return - - self.send_rally_points() - self.have_list = True - - print("Loaded %u rally points from %s" % (self.rallyloader.rally_count(), args[1])) - - elif args[0] == "save": - if (len(args) < 2): - print("Usage: rally save filename") - return - - self.rallyloader.save(args[1].strip('"')) - - print("Saved rally file %s" % args[1]) - - elif args[0] == "alt": - self.cmd_rally_alt(args[1:]) - - elif args[0] == "land": - if (len(args) >= 2 and args[1] == "abort"): - self.abort_ack_received = False - self.abort_first_send_time = 0 - - self.abort_alt = self.settings.rally_breakalt - if (len(args) >= 3): - self.abort_alt = int(args[2]) - - else: - self.master.mav.command_long_send(self.settings.target_system, - self.settings.target_component, - mavutil.mavlink.MAV_CMD_DO_RALLY_LAND, - 0, 0, 0, 0, 0, 0, 0, 0) - + alt = self.settings.rallyalt else: - self.print_usage() - - def mavlink_packet(self, m): - '''handle incoming mavlink packet''' - type = m.get_type() - if type in ['COMMAND_ACK']: - if m.command == mavutil.mavlink.MAV_CMD_DO_GO_AROUND: - if (m.result == 0 and self.abort_ack_received == False): - self.say("Landing Abort Command Successfully Sent.") - self.abort_ack_received = True - elif (m.result != 0 and self.abort_ack_received == False): - self.say("Landing Abort Command Unsuccessful.") - - elif m.command == mavutil.mavlink.MAV_CMD_DO_RALLY_LAND: - if (m.result == 0): - self.say("Landing.") - - def unload(self): - self.remove_command("rally") - 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(RallyModule, self).unload() - - def send_rally_point(self, i): - '''send rally points from fenceloader''' - p = self.rallyloader.rally_point(i) - p.target_system = self.target_system - p.target_component = self.target_component - self.master.mav.send(p) - - def send_rally_points(self): - '''send rally points from rallyloader''' - self.mav_param.mavset(self.master,'RALLY_TOTAL',self.rallyloader.rally_count(),3) + alt = float(args[0]) - for i in range(self.rallyloader.rally_count()): - self.send_rally_point(i) + 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 fetch_rally_point(self, i): - '''fetch one rally point''' - self.master.mav.rally_fetch_point_send(self.target_system, - self.target_component, i) - tstart = time.time() - p = None - while time.time() - tstart < 1: - p = self.master.recv_match(type='RALLY_POINT', blocking=False) - if p is not None: - break - time.sleep(0.1) - continue - if p is None: - self.console.error("Failed to fetch rally point %u" % i) + 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 - return p - - def list_rally_points(self): - self.rallyloader.clear() - rally_count = self.mav_param.get('RALLY_TOTAL',0) - if rally_count == 0: - print("No rally points") - return - for i in range(int(rally_count)): - p = self.fetch_rally_point(i) - if p is None: - return - self.rallyloader.append_rally_point(p) + 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) - for i in range(self.rallyloader.rally_count()): - p = self.rallyloader.rally_point(i) - self.console.writeln("lat=%f lng=%f alt=%f break_alt=%f land_dir=%f autoland=%f" % (p.lat * 1e-7, p.lng * 1e-7, p.alt, p.break_alt, p.land_dir, int(p.flags & 2!=0) )) - - if self.logdir is not None: - fname = 'ral.txt' - if self.target_system > 1: - fname = 'ral_%u.txt' % self.target_system - ral_file_path = os.path.join(self.logdir, fname) - self.rallyloader.save(ral_file_path) - print("Saved rally points to %s" % ral_file_path) + 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 print_usage(self): - print("Usage: rally ") def init(mpstate): '''initialise module''' + + # see if pymavlink is new enough to support new protocols: + oldmodule = "rallypoint_protocol" + try: + mavwp.MissionItemProtocol_Rally + except AttributeError: + print("pymavlink too old; using old %s module" % oldmodule) + mpstate.load_module(oldmodule) + for (m, pm) in mpstate.modules: + if m.name == "rally": + return m + return None + return RallyModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_rallypoint_protocol.py b/MAVProxy/modules/mavproxy_rallypoint_protocol.py new file mode 100644 index 0000000000..aaa90f3589 --- /dev/null +++ b/MAVProxy/modules/mavproxy_rallypoint_protocol.py @@ -0,0 +1,360 @@ +""" + MAVProxy rally module +""" + +from pymavlink import mavwp +from pymavlink import mavutil +import time, os, platform +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 * + +class RallyModule(mp_module.MPModule): + def __init__(self, mpstate): + super(RallyModule, self).__init__(mpstate, "rally", "rally point control", public = True) + self.rallyloader_by_sysid = {} + self.add_command('rally', self.cmd_rally, "old rally point control", ["", + " (FILENAME)"]) + self.have_list = False + self.abort_alt = 50 + self.abort_first_send_time = 0 + self.abort_previous_send_time = 0 + self.abort_ack_received = True + + self.menu_added_console = False + self.menu_added_map = False + if mp_util.has_wxpython: + self.menu = MPMenuSubMenu('Rally', + items=[MPMenuItem('Clear', 'Clear', '# rally clear'), + MPMenuItem('List', 'List', '# rally list'), + MPMenuItem('Load', 'Load', '# rally load ', + handler=MPMenuCallFileDialog(flags=('open',), + title='Rally Load', + wildcard='RallyPoints(*.txt,*.rally,*.ral)|*.txt;*.rally;*.ral')), + MPMenuItem('Save', 'Save', '# rally save ', + handler=MPMenuCallFileDialog(flags=('save', 'overwrite_prompt'), + title='Rally Save', + wildcard='RallyPoints(*.txt,*.rally,*.ral)|*.txt;*.rally;*.ral')), + MPMenuItem('Add', 'Add', '# rally add ', + handler=MPMenuCallTextDialog(title='Rally Altitude (m)', + default=100))]) + + @property + def rallyloader(self): + '''rally loader by system ID''' + if not self.target_system in self.rallyloader_by_sysid: + self.rallyloader_by_sysid[self.target_system] = mavwp.MAVRallyLoader(self.settings.target_system, + self.settings.target_component) + return self.rallyloader_by_sysid[self.target_system] + + def last_change(self): + '''return time of last changes made to rally points''' + return self.rallyloader.last_change + + def rally_count(self): + '''return number of waypoints''' + return self.rallyloader.rally_count() + + def rally_point(self, i): + '''return instance of mavutil.mavlink.MAVLink_rally_point_message''' + return self.rallyloader.rally_point(i) + + def set_last_change(self, time): + '''can be used to cause map redraws''' + self.rallyloader.last_change = time + + def idle_task(self): + '''called on idle''' + 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 + + '''handle abort command; it is critical that the AP to receive it''' + if self.abort_ack_received is False: + #only send abort every second (be insistent, but don't spam) + if (time.time() - self.abort_previous_send_time > 1): + self.master.mav.command_long_send(self.settings.target_system, + self.settings.target_component, + mavutil.mavlink.MAV_CMD_DO_GO_AROUND, + 0, int(self.abort_alt), 0, 0, 0, 0, 0, 0,) + self.abort_previous_send_time = time.time() + + #try to get an ACK from the plane: + if self.abort_first_send_time == 0: + self.abort_first_send_time = time.time() + elif time.time() - self.abort_first_send_time > 10: #give up after 10 seconds + print("Unable to send abort command!\n") + self.abort_ack_received = True + + + def cmd_rally_add(self, args): + '''handle rally add''' + if len(args) < 1: + alt = self.settings.rallyalt + else: + alt = float(args[0]) + + if len(args) < 2: + break_alt = self.settings.rally_breakalt + else: + break_alt = float(args[1]) + + if len(args) < 3: + flag = self.settings.rally_flags + else: + flag = int(args[2]) + #currently only supporting autoland values: + #True (nonzero) and False (zero) + if (flag != 0): + flag = 2 + + if not self.have_list: + print("Please list rally points first") + return + + if (self.rallyloader.rally_count() > 4): + print("Only 5 rally points possible per flight plan.") + return + + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + + land_hdg = 0.0 + + self.rallyloader.create_and_append_rally_point(latlon[0] * 1e7, latlon[1] * 1e7, alt, break_alt, land_hdg, flag) + self.send_rally_points() + print("Added Rally point at %s %f %f, autoland: %s" % (str(latlon), alt, break_alt, bool(flag & 2))) + + def cmd_rally_alt(self, args): + '''handle rally alt change''' + if (len(args) < 2): + print("Usage: rally alt RALLYNUM newAlt ") + return + if not self.have_list: + print("Please list rally points first") + return + + idx = int(args[0]) + if idx <= 0 or idx > self.rallyloader.rally_count(): + print("Invalid rally point number %u" % idx) + return + + new_alt = int(args[1]) + new_break_alt = None + if (len(args) > 2): + new_break_alt = int(args[2]) + + self.rallyloader.set_alt(idx, new_alt, new_break_alt) + self.send_rally_point(idx-1) + self.fetch_rally_point(idx-1) + self.rallyloader.reindex() + + def cmd_rally_move(self, args): + '''handle rally move''' + if len(args) < 1: + print("Usage: rally move RALLYNUM") + return + if not self.have_list: + print("Please list rally points first") + return + + idx = int(args[0]) + if idx <= 0 or idx > self.rallyloader.rally_count(): + print("Invalid rally point number %u" % idx) + return + + rpoint = self.rallyloader.rally_point(idx-1) + + latlon = self.mpstate.click_location + if latlon is None: + print("No map click position available") + return + + oldpos = (rpoint.lat*1e-7, rpoint.lng*1e-7) + self.rallyloader.move(idx, latlon[0], latlon[1]) + self.send_rally_point(idx-1) + p = self.fetch_rally_point(idx-1) + if p.lat != int(latlon[0]*1e7) or p.lng != int(latlon[1]*1e7): + print("Rally move failed") + return + self.rallyloader.reindex() + print("Moved rally point from %s to %s at %fm" % (str(oldpos), str(latlon), rpoint.alt)) + + + def cmd_rally(self, args): + '''rally point commands''' + #TODO: add_land arg + if len(args) < 1: + self.print_usage() + return + + elif args[0] == "add": + self.cmd_rally_add(args[1:]) + + elif args[0] == "move": + self.cmd_rally_move(args[1:]) + + elif args[0] == "clear": + self.rallyloader.clear() + self.mav_param.mavset(self.master,'RALLY_TOTAL',0,3) + + elif args[0] == "remove": + if not self.have_list: + print("Please list rally points first") + return + if (len(args) < 2): + print("Usage: rally remove RALLYNUM") + return + self.rallyloader.remove(int(args[1])) + self.send_rally_points() + + elif args[0] == "list": + self.list_rally_points() + self.have_list = True + + elif args[0] == "load": + if (len(args) < 2): + print("Usage: rally load filename") + return + + try: + self.rallyloader.load(args[1].strip('"')) + except Exception as msg: + print("Unable to load %s - %s" % (args[1], msg)) + return + + self.send_rally_points() + self.have_list = True + + print("Loaded %u rally points from %s" % (self.rallyloader.rally_count(), args[1])) + + elif args[0] == "save": + if (len(args) < 2): + print("Usage: rally save filename") + return + + self.rallyloader.save(args[1].strip('"')) + + print("Saved rally file %s" % args[1]) + + elif args[0] == "alt": + self.cmd_rally_alt(args[1:]) + + elif args[0] == "land": + if (len(args) >= 2 and args[1] == "abort"): + self.abort_ack_received = False + self.abort_first_send_time = 0 + + self.abort_alt = self.settings.rally_breakalt + if (len(args) >= 3): + self.abort_alt = int(args[2]) + + else: + self.master.mav.command_long_send(self.settings.target_system, + self.settings.target_component, + mavutil.mavlink.MAV_CMD_DO_RALLY_LAND, + 0, 0, 0, 0, 0, 0, 0, 0) + + else: + self.print_usage() + + def mavlink_packet(self, m): + '''handle incoming mavlink packet''' + type = m.get_type() + if type in ['COMMAND_ACK']: + if m.command == mavutil.mavlink.MAV_CMD_DO_GO_AROUND: + if (m.result == 0 and self.abort_ack_received == False): + self.say("Landing Abort Command Successfully Sent.") + self.abort_ack_received = True + elif (m.result != 0 and self.abort_ack_received == False): + self.say("Landing Abort Command Unsuccessful.") + + elif m.command == mavutil.mavlink.MAV_CMD_DO_RALLY_LAND: + if (m.result == 0): + self.say("Landing.") + + def unload(self): + self.remove_command("rally") + 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(RallyModule, self).unload() + + def send_rally_point(self, i): + '''send rally points from fenceloader''' + p = self.rallyloader.rally_point(i) + p.target_system = self.target_system + p.target_component = self.target_component + self.master.mav.send(p) + + def send_rally_points(self): + '''send rally points from rallyloader''' + self.mav_param.mavset(self.master,'RALLY_TOTAL',self.rallyloader.rally_count(),3) + + for i in range(self.rallyloader.rally_count()): + self.send_rally_point(i) + + def fetch_rally_point(self, i): + '''fetch one rally point''' + self.master.mav.rally_fetch_point_send(self.target_system, + self.target_component, i) + tstart = time.time() + p = None + while time.time() - tstart < 1: + p = self.master.recv_match(type='RALLY_POINT', blocking=False) + if p is not None: + break + time.sleep(0.1) + continue + if p is None: + self.console.error("Failed to fetch rally point %u" % i) + return None + return p + + def list_rally_points(self): + self.rallyloader.clear() + rally_count = self.mav_param.get('RALLY_TOTAL',0) + if rally_count == 0: + print("No rally points") + return + for i in range(int(rally_count)): + p = self.fetch_rally_point(i) + if p is None: + return + self.rallyloader.append_rally_point(p) + + for i in range(self.rallyloader.rally_count()): + p = self.rallyloader.rally_point(i) + self.console.writeln("lat=%f lng=%f alt=%f break_alt=%f land_dir=%f autoland=%f" % (p.lat * 1e-7, p.lng * 1e-7, p.alt, p.break_alt, p.land_dir, int(p.flags & 2!=0) )) + + if self.logdir is not None: + fname = 'ral.txt' + if self.target_system > 1: + fname = 'ral_%u.txt' % self.target_system + ral_file_path = os.path.join(self.logdir, fname) + self.rallyloader.save(ral_file_path) + print("Saved rally points to %s" % ral_file_path) + + def print_usage(self): + print("Usage: rally ") + +def init(mpstate): + '''initialise module''' + return RallyModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index d1bd911074..91d8f45f7a 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()