Add support for MAV_CMD_DO_ORBIT

Author: peterbarker

MAVProxy/modules/lib/mp_menu.py

@@ -450,6 +450,124 @@ def call(self):
         # Return tuple with text value and selected dropdown value
         return text_value + " " + dropdown_value
 
+class MPMenuCallMultiTextDropdownDialog(object):
+    '''used to create a dialog callback with multiple values and optional dropdowns.
+
+    Each field is a (label, default) tuple, and each dropdown a
+    (label, options, default) tuple.  An extra trailing element - a key -
+    may be added to either (i.e. (label, default, key) or
+    (label, options, default, key)); when present the corresponding value
+    is emitted as "key=value", otherwise the bare value is emitted.
+    '''
+    def __init__(self, title='Enter Values', fields=None, dropdowns=None):
+        self.title = title
+        self.fields = fields or []
+        self.dropdowns = dropdowns or []
+
+    @classmethod
+    def from_arg_spec(cls, title, spec, ctx=None):
+        '''build a dialog from a command argument spec.
+
+        spec is an ordered dict of key->facets (see
+        MPModule.parse_key_value_spec).  Each entry that carries a 'label'
+        becomes a widget emitting "key=value": a dropdown when it has an
+        'options' list, otherwise a text field.  Entries without a 'label'
+        (e.g. keys entered another way) are skipped.  A 'default' facet may
+        be a plain value or a callable which is passed ctx to produce a
+        live default.
+        '''
+        fields = []
+        dropdowns = []
+        for (key, facets) in spec.items():
+            if 'label' not in facets:
+                continue
+            default = facets.get('default', '')
+            if callable(default):
+                default = default(ctx)
+            if 'options' in facets:
+                dropdowns.append((facets['label'], facets['options'], default, key))
+            else:
+                fields.append((facets['label'], default, key))
+        return cls(title=title, fields=fields, dropdowns=dropdowns)
+
+    def call(self):
+        '''show a dialog with multiple value entries and optional dropdowns'''
+        from MAVProxy.modules.lib.wx_loader import wx
+
+        # Create a custom dialog
+        dlg = wx.Dialog(None, title=self.title, size=(400, 150))
+
+        # Create a vertical box sizer for the dialog
+        main_sizer = wx.BoxSizer(wx.VERTICAL)
+
+        # One row of label plus text entry per field
+        text_ctrls = []
+        for field in self.fields:
+            (label, default) = field[0], field[1]
+            input_sizer = wx.BoxSizer(wx.HORIZONTAL)
+            text_label = wx.StaticText(dlg, label=label + ":")
+            memory_key = self.title + ":" + label
+            default = last_value_selection.get(memory_key, default)
+            text_ctrl = wx.TextCtrl(dlg, value=str(default), size=(200, -1))
+            text_ctrls.append(text_ctrl)
+            input_sizer.Add(text_label, 1, wx.ALL | wx.ALIGN_CENTER_VERTICAL, 5)
+            input_sizer.Add(text_ctrl, 0, wx.ALL | wx.EXPAND, 5)
+            main_sizer.Add(input_sizer, 0, wx.ALL | wx.EXPAND, 5)
+
+        # One row of label plus choice control per dropdown
+        dropdown_ctrls = []
+        for dropdown in self.dropdowns:
+            (label, options, default) = dropdown[0], dropdown[1], dropdown[2]
+            input_sizer = wx.BoxSizer(wx.HORIZONTAL)
+            dropdown_label = wx.StaticText(dlg, label=label)
+            dropdown_ctrl = wx.Choice(dlg, choices=options)
+            dropdown_ctrls.append(dropdown_ctrl)
+
+            default_idx = 0
+            for i in range(len(options)):
+                if options[i] == default:
+                    default_idx = i
+            memory_key = self.title + ":" + label
+            dropdown_ctrl.SetSelection(last_dropdown_selection.get(memory_key, default_idx))
+
+            input_sizer.Add(dropdown_label, 1, wx.ALL | wx.ALIGN_CENTER_VERTICAL, 5)
+            input_sizer.Add(dropdown_ctrl, 0, wx.ALL | wx.EXPAND, 5)
+            main_sizer.Add(input_sizer, 0, wx.ALL | wx.EXPAND, 5)
+
+        # Create button sizer with OK and Cancel buttons
+        button_sizer = dlg.CreateButtonSizer(wx.OK | wx.CANCEL)
+        main_sizer.Add(button_sizer, 0, wx.ALL | wx.ALIGN_CENTER, 10)
+
+        # Set the sizer for the dialog
+        dlg.SetSizer(main_sizer)
+
+        # Fit the dialog to its contents
+        dlg.Fit()
+
+        # Show the dialog and get the result
+        if dlg.ShowModal() != wx.ID_OK:
+            return None
+
+        values = []
+        for (field, text_ctrl) in zip(self.fields, text_ctrls):
+            label = field[0]
+            key = field[2] if len(field) > 2 else None
+            text_value = text_ctrl.GetValue()
+            last_value_selection[self.title + ":" + label] = text_value
+            values.append("%s=%s" % (key, text_value) if key else text_value)
+
+        for (dropdown, dropdown_ctrl) in zip(self.dropdowns, dropdown_ctrls):
+            (label, options) = dropdown[0], dropdown[1]
+            key = dropdown[3] if len(dropdown) > 3 else None
+            dropdown_index = dropdown_ctrl.GetSelection()
+            if dropdown_index != -1:
+                dropdown_value = options[dropdown_index]
+                last_dropdown_selection[self.title + ":" + label] = dropdown_index
+                values.append("%s=%s" % (key, dropdown_value) if key else dropdown_value)
+
+        # Return the entered values, space-separated, dropdown values last
+        return " ".join(values)
+
 class MPMenuConfirmDialog(object):
     '''used to create a confirmation dialog'''
     def __init__(self, title='Confirmation', message='', callback=None, args=None):

MAVProxy/modules/lib/mp_module.py

@@ -157,6 +157,82 @@ def remove_command(self, name):
     def add_completion_function(self, name, callback):
         self.mpstate.completion_functions[name] = callback
 
+    def parse_key_value_args(self, args, valid_keys, values):
+        '''split "key=value" arguments into the values dict.
+
+        valid_keys maps each accepted (lower-case) key to a callable used
+        to convert its value (e.g. int, float, str), so values ends up
+        holding correctly-typed entries.  Returns True on success, or
+        prints an error and returns False for an argument which is not in
+        key=value form, an unknown key, or a value which will not convert.
+        '''
+        for arg in args:
+            if '=' not in arg:
+                print("Argument '%s' is not in key=value form" % arg)
+                return False
+            (key, value) = arg.split('=', 1)
+            key = key.lower()
+            if key not in valid_keys:
+                print("Unknown argument '%s'" % key)
+                return False
+            try:
+                values[key] = valid_keys[key](value)
+            except ValueError:
+                print("Invalid value '%s' for argument '%s'" % (value, key))
+                return False
+        return True
+
+    def parse_key_value_spec(self, args, spec, values):
+        '''parse "key=value" arguments according to an argument spec.
+
+        spec is an ordered dict mapping each canonical key to a dict of
+        facets; the facets used here are:
+          type      callable used to convert the value (required)
+          synonyms  list of alternative spellings folded onto this key
+          required  if True, the key must be supplied
+        Converted, synonym-folded values are stored in the values dict.
+        Returns True on success, or prints an error and returns False.
+        See parse_key_value_args() and format_key_value_help().
+        '''
+        valid_keys = {}
+        for (key, facets) in spec.items():
+            valid_keys[key] = facets['type']
+            for synonym in facets.get('synonyms', []):
+                valid_keys[synonym] = facets['type']
+
+        if not self.parse_key_value_args(args, valid_keys, values):
+            return False
+
+        # fold each synonym onto its canonical key
+        for (key, facets) in spec.items():
+            for synonym in facets.get('synonyms', []):
+                if synonym in values:
+                    values[key] = values.pop(synonym)
+
+        for (key, facets) in spec.items():
+            if facets.get('required') and key not in values:
+                print("%s is required" % key)
+                return False
+        return True
+
+    def format_key_value_help(self, spec):
+        '''return a list of aligned description lines for an argument spec.
+
+        Each line describes one key (with any synonyms) and its help text,
+        marking required keys.  See parse_key_value_spec().
+        '''
+        names = {}
+        for (key, facets) in spec.items():
+            names[key] = '/'.join([key] + facets.get('synonyms', []))
+        width = max(len(n) for n in names.values())
+        lines = []
+        for (key, facets) in spec.items():
+            text = facets.get('help', '')
+            if facets.get('required'):
+                text += ' (required)'
+            lines.append("  %-*s  %s" % (width, names[key], text))
+        return lines
+
     def flyto_frame_units(self):
         '''return a frame string and unit'''
         return "%s %s" % (self.settings.height_unit, self.settings.flytoframe)

MAVProxy/modules/mavproxy_map/__init__.py

@@ -14,6 +14,7 @@
 from MAVProxy.modules.lib import mp_settings
 from MAVProxy.modules.lib import mp_module
 from MAVProxy.modules.lib.mp_menu import *
+from MAVProxy.modules.mavproxy_mode import orbit_arg_spec
 from pymavlink import mavutil
 from PIL import ImageColor
 
@@ -120,6 +121,15 @@ def __init__(self, mpstate):
                 default_dropdown=self.settings.flytoframe,
             )
         ))
+        self.add_menu(MPMenuItem(
+            'Orbit Here', 'Orbit Here', '# orbit ',
+            handler=MPMenuCallMultiTextDropdownDialog.from_arg_spec(
+                'Orbit', orbit_arg_spec,
+                ctx={
+                    'guidedalt': self.mpstate.settings.guidedalt,
+                    'flytoframe': self.settings.flytoframe,
+                })
+        ))
         self.add_menu(MPMenuItem('Terrain Check', 'Terrain Check', '# terrain check'))
         self.add_menu(MPMenuItem('Show Position', 'Show Position', 'showPosition'))
         self.add_menu(MPMenuItem('Google Maps Link', 'Google Maps Link', 'printGoogleMapsLink'))

MAVProxy/modules/mavproxy_mode.py

@@ -1,6 +1,9 @@
 #!/usr/bin/env python3
 '''mode command handling'''
 
+import collections
+import math
+
 from pymavlink import mavutil
 
 from MAVProxy.modules.lib import mp_module
@@ -10,6 +13,52 @@
 AP_FLAKE8_CLEAN
 '''
 
+# names for MAV_CMD_DO_ORBIT param3, ORBIT_YAW_BEHAVIOUR values; NaN
+# leaves the choice to the vehicle
+orbit_yaw_behaviours = {
+    "Default": float('NaN'),
+    "FaceCentre": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER,
+    "InitialHeading": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING,
+    "Uncontrolled": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_UNCONTROLLED,
+    "Tangent": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TANGENT_TO_CIRCLE,
+    "RCControlled": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_RC_CONTROLLED,
+    "Unchanged": mavutil.mavlink.ORBIT_YAW_BEHAVIOUR_UNCHANGED,
+}
+
+orbit_frames = ['AboveHome', 'AGL', 'AMSL']
+
+# single source of truth for the "orbit" command arguments: drives
+# command-line parsing, command-line help, and the map "Orbit Here" popup
+# dialog.  See MPModule.parse_key_value_spec / format_key_value_help and
+# MPMenuCallMultiTextDropdownDialog.from_arg_spec for the facets consumed.
+# Keys without a 'label' (loc/lat/lng) are command-line only: the popup
+# takes its centre from the map click.
+orbit_arg_spec = collections.OrderedDict([
+    ('radius', dict(type=float, required=True,
+                    help='metres, positive for clockwise, negative for counter-clockwise',
+                    label='Radius (m, -ve for CCW)', default=50)),
+    ('alt', dict(type=float, synonyms=['altitude'],
+                 help='orbit altitude',
+                 label='Altitude', default=lambda ctx: ctx['guidedalt'])),
+    ('velocity', dict(type=str,
+                      help="tangential velocity in m/s, 'default' for the vehicle default",
+                      label='Velocity (m/s)', default='Default')),
+    ('orbits', dict(type=float,
+                    help='number of circuits to fly, 0 to orbit forever',
+                    label='Orbits (0 for forever)', default=0)),
+    ('yaw', dict(type=str, options=list(orbit_yaw_behaviours.keys()),
+                 help='one of %s or an ORBIT_YAW_BEHAVIOUR enumeration value' %
+                      '|'.join(orbit_yaw_behaviours.keys()),
+                 label='Yaw Behaviour', default='Default')),
+    ('frame', dict(type=str, options=orbit_frames,
+                   help='altitude frame',
+                   label='Frame', default=lambda ctx: ctx['flytoframe'])),
+    ('loc', dict(type=str,
+                 help='orbit centre as LAT,LNG or LAT,LNG,ALT; defaults to the map click position')),
+    ('lat', dict(type=float, synonyms=['latitude'], help='orbit centre latitude')),
+    ('lng', dict(type=float, synonyms=['longitude', 'lon'], help='orbit centre longitude')),
+])
+
 
 class ModeModule(mp_module.MPModule):
     def __init__(self, mpstate):
@@ -18,6 +67,7 @@ def __init__(self, mpstate):
             '(MODE)'
         ])
         self.add_command('guided', self.cmd_guided, "fly to a clicked location on map")
+        self.add_command('orbit', self.cmd_orbit, "orbit around a clicked location on map")
         self.add_command('confirm', self.cmd_confirm, "confirm a command")
         self.add_completion_function('(MODE)', self.complete_available_modes)
 
@@ -140,6 +190,93 @@ def cmd_guided(self, args):
             altitude
         )
 
+    def cmd_orbit_usage(self):
+        '''print usage for the orbit command'''
+        print("Usage: orbit radius=RADIUS [alt=ALTITUDE] [velocity=VELOCITY] "
+              "[orbits=ORBITS] [yaw=YAW] [frame=%s]" % '|'.join(orbit_frames))
+        print("            [loc=LAT,LNG | lat=LAT lng=LNG]")
+        for line in self.format_key_value_help(orbit_arg_spec):
+            print(line)
+
+    def cmd_orbit(self, args):
+        '''send MAV_CMD_DO_ORBIT to orbit around the clicked location'''
+        values = {}
+        if not self.parse_key_value_spec(args, orbit_arg_spec, values):
+            self.cmd_orbit_usage()
+            return
+
+        # work out the orbit centre: an explicit location if given, else the map click
+        if 'loc' in values:
+            if 'lat' in values or 'lng' in values:
+                print("specify either loc= or lat=/lng=, not both")
+                return
+            parts = values['loc'].split(',')
+            try:
+                if len(parts) not in (2, 3):
+                    raise ValueError
+                latlon = (float(parts[0]), float(parts[1]))
+                if len(parts) == 3 and 'alt' not in values:
+                    values['alt'] = float(parts[2])
+            except ValueError:
+                print("loc must be LAT,LNG or LAT,LNG,ALT")
+                return
+        elif 'lat' in values or 'lng' in values:
+            if 'lat' not in values or 'lng' not in values:
+                print("both lat and lng are required")
+                return
+            latlon = (values['lat'], values['lng'])
+        else:
+            latlon = self.mpstate.click_location
+            if latlon is None:
+                print("No map click position available")
+                return
+
+        if 'frame' in values:
+            if values['frame'] not in orbit_frames:
+                print("frame must be one of %s" % '|'.join(orbit_frames))
+                return
+            self.settings.flytoframe = values['frame']
+
+        radius = values['radius']
+        if 'alt' in values:
+            altitude = values['alt']
+        else:
+            altitude = self.mpstate.settings.guidedalt
+        altitude = self.height_convert_from_units(altitude)
+        velocity = float('NaN')  # vehicle default velocity
+        if 'velocity' in values and values['velocity'].lower() != 'default':
+            velocity = float(values['velocity'])
+        orbits = 0  # orbit forever
+        if 'orbits' in values:
+            orbits = values['orbits']
+        yaw_behaviour = float('NaN')  # vehicle default yaw behaviour
+        if 'yaw' in values:
+            behaviours_lower = {k.lower(): v for (k, v) in orbit_yaw_behaviours.items()}
+            if values['yaw'].lower() in behaviours_lower:
+                yaw_behaviour = behaviours_lower[values['yaw'].lower()]
+            else:
+                yaw_behaviour = float(values['yaw'])
+
+        frame = self.flyto_frame()
+
+        print("Orbit %s radius %.1fm alt %.1f frame %u" % (str(latlon), radius, altitude, frame))
+
+        self.master.mav.command_int_send(
+            self.settings.target_system,
+            self.settings.target_component,
+            frame,
+            mavutil.mavlink.MAV_CMD_DO_ORBIT,
+            0,  # current
+            0,  # autocontinue
+            radius,  # p1 - radius, +ve clockwise, -ve counter-clockwise
+            velocity,  # p2 - tangential velocity, NaN is use-default
+            yaw_behaviour,  # p3 - yaw behaviour, NaN is vehicle default
+            orbits * 2 * math.pi,  # p4 - angle to orbit in radians, 0 for forever
+            int(latlon[0]*1.0e7),
+            int(latlon[1]*1.0e7),
+            altitude
+        )
+
     def build_pt_ignoremask(self, bits, force_not_accel=False):
         '''creates an ignore bitmask which ignores all bits except the ones passed in'''
         ignore_bits = {