From 7893643af19cbd730c5c8d9eddaa377f08c57c7b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 26 Mar 2024 13:24:18 +1100 Subject: [PATCH] added flytoframe setting allows guided AGL --- MAVProxy/mavproxy.py | 1 + MAVProxy/modules/lib/mp_menu.py | 11 ++++++++--- MAVProxy/modules/lib/mp_module.py | 19 +++++++++++++++++++ MAVProxy/modules/mavproxy_map/__init__.py | 3 ++- MAVProxy/modules/mavproxy_mode.py | 10 +++++++--- 5 files changed, 37 insertions(+), 7 deletions(-) diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index 6c2fe2b0f7..738303d25f 100644 --- a/MAVProxy/mavproxy.py +++ b/MAVProxy/mavproxy.py @@ -281,6 +281,7 @@ def __init__(self): MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'), MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']), MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']), + MPSetting('flytoframe', str, 'AboveHome', 'frame for FlyTo', choice=['AboveHome', 'AGL', 'AMSL']), MPSetting('fwdpos', bool, False, 'Forward GLOBAL_POSITION_INT on all links'), MPSetting('checkdelay', bool, True, 'check for link delay'), diff --git a/MAVProxy/modules/lib/mp_menu.py b/MAVProxy/modules/lib/mp_menu.py index d2a25e4537..732b6b83d4 100644 --- a/MAVProxy/modules/lib/mp_menu.py +++ b/MAVProxy/modules/lib/mp_menu.py @@ -336,17 +336,22 @@ def call(self): class MPMenuCallTextDialog(object): '''used to create a value dialog callback''' - def __init__(self, title='Enter Value', default=''): + def __init__(self, title='Enter Value', default='', settings=None): self.title = title self.default = default + self.settings = settings def call(self): '''show a value dialog''' from MAVProxy.modules.lib.wx_loader import wx + title = self.title + if title.find('FLYTOFRAMEUNITS') != -1 and self.settings is not None: + frameunits = "%s %s" % (self.settings.flytoframe, self.settings.height_unit) + title = title.replace('FLYTOFRAMEUNITS', frameunits) try: - dlg = wx.TextEntryDialog(None, self.title, self.title, defaultValue=str(self.default)) + dlg = wx.TextEntryDialog(None, title, title, defaultValue=str(self.default)) except TypeError: - dlg = wx.TextEntryDialog(None, self.title, self.title, value=str(self.default)) + dlg = wx.TextEntryDialog(None, title, title, value=str(self.default)) if dlg.ShowModal() != wx.ID_OK: return None return dlg.GetValue() diff --git a/MAVProxy/modules/lib/mp_module.py b/MAVProxy/modules/lib/mp_module.py index 832c7a9be3..8c84026462 100644 --- a/MAVProxy/modules/lib/mp_module.py +++ b/MAVProxy/modules/lib/mp_module.py @@ -1,4 +1,5 @@ import time +from pymavlink import mavutil class MPModule(object): ''' @@ -153,6 +154,18 @@ def remove_command(self, name): def add_completion_function(self, name, callback): self.mpstate.completion_functions[name] = callback + def flyto_frame_units(self): + '''return a frame string and unit''' + return "%s %s" % (self.settings.height_unit, self.settings.flytoframe) + + def flyto_frame(self): + '''return mavlink frame flyto frame setting''' + if self.settings.flytoframe == "AGL": + return mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT + if self.settings.flytoframe == "AMSL": + return mavutil.mavlink.MAV_FRAME_GLOBAL + return mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + def dist_string(self, val_meters): '''return a distance as a string''' if self.settings.dist_unit == 'nm': @@ -167,6 +180,12 @@ def height_convert_units(self, val_meters): return val_meters * 3.28084 return val_meters + def height_convert_from_units(self, val): + '''convert a height from configured units''' + if self.settings.height_unit == 'feet': + return val / 3.28084 + return val + def height_string(self, val_meters): '''return a height as a string''' if self.settings.height_unit == 'feet': diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 05c011df15..43ae6fe8c2 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -92,7 +92,8 @@ def __init__(self, mpstate): self.default_popup = MPMenuSubMenu('Popup', items=[]) self.add_menu(MPMenuItem('Fly To', 'Fly To', '# guided ', - handler=MPMenuCallTextDialog(title='Altitude (m)', default=self.mpstate.settings.guidedalt))) + handler=MPMenuCallTextDialog(title='Altitude (FLYTOFRAMEUNITS)', default=self.mpstate.settings.guidedalt, + settings=self.settings))) self.add_menu(MPMenuItem('Set Home', 'Set Home', '# confirm "Set HOME?" map sethomepos ')) self.add_menu(MPMenuItem('Set Home (with height)', 'Set Home', '# confirm "Set HOME with height?" map sethome ')) self.add_menu(MPMenuItem('Set Origin', 'Set Origin', '# confirm "Set ORIGIN?" map setoriginpos ')) diff --git a/MAVProxy/modules/mavproxy_mode.py b/MAVProxy/modules/mavproxy_mode.py index 8b715ae938..99029ef7a6 100644 --- a/MAVProxy/modules/mavproxy_mode.py +++ b/MAVProxy/modules/mavproxy_mode.py @@ -83,13 +83,17 @@ def cmd_guided(self, args): return altitude = float(args[0]) - print("Guided %s %s" % (str(latlon), str(altitude))) + altitude = self.height_convert_from_units(altitude) + + frame = self.flyto_frame() + + print("Guided %s %s frame %u" % (str(latlon), str(altitude), frame)) if self.settings.guided_use_reposition: self.master.mav.command_int_send( self.settings.target_system, self.settings.target_component, - self.module('wp').get_default_frame(), + frame, mavutil.mavlink.MAV_CMD_DO_REPOSITION, 0, # current 0, # autocontinue @@ -107,7 +111,7 @@ def cmd_guided(self, args): self.settings.target_system, self.settings.target_component, 0, - self.module('wp').get_default_frame(), + frame, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 0, 0, 0, int(latlon[0]*1.0e7),