added flytoframe setting

allows guided AGL
tridge · Mar 26, 2024 · 7893643 · 7893643
1 parent 7d45a82
commit 7893643
Show file tree

Hide file tree

Showing 5 changed files with 37 additions and 7 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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),