SIYI: latest updates from flight tests

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'),

MAVProxy/modules/lib/mission_item_protocol.py

@@ -694,6 +694,8 @@ def cmd_changealt(self, args):
         for wpnum in range(idx, idx+count):
             offset = self.item_num_to_offset(wpnum)
             wp = self.wploader.wp(offset)
+            if wp is None:
+                continue
             if not self.wploader.is_location_command(wp.command):
                 continue
             wp.z = newalt
@@ -712,6 +714,49 @@ def cmd_changealt(self, args):
             mission_type=self.mav_mission_type())
         print("Changed alt for WPs %u:%u to %f" % (idx, idx+(count-1), newalt))
 
+    def cmd_changeframe(self, args):
+        '''handle wp change frame of multiple waypoints'''
+        if not self.check_have_list():
+            return
+        if len(args) < 2:
+            print("usage: %s changeframe WPNUM NEWFRAME <NUMWP>" % 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
+        newframe = int(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 wp is None:
+                continue
+            if not self.wploader.is_location_command(wp.command):
+                continue
+            wp.frame = newframe
+            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 frame for WPs %u:%u to %u" % (idx, idx+(count-1), newframe))
+
     def fix_jumps(self, idx, delta):
         # nothing by default as only waypoints need worry
         pass

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()

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':