teach mavproxy about new "joymap" module

This is an alternative to the existing "joystick" module.  Unlike the
joystick module, it loads joystick definitions dynamically from YAML
files, which makes it easier for someone to add support for a device
without needing to patch mavproxy itself.

Additionally, the "joymap" module supports a wider variety of control
types.  A "hat" can be used as a pair of toggle switches, and buttons
can be used either as momentary switches or grouped together to act
like a multi-position switch.

MAVProxy/modules/mavproxy_joystick/__init__.py

@@ -0,0 +1,157 @@
+from __future__ import print_function
+
+import os
+import pygame
+import pkg_resources
+import yaml
+import fnmatch
+
+from MAVProxy.modules.lib import mp_module
+from MAVProxy.modules.lib import mp_util
+from MAVProxy.modules.lib import mp_settings
+
+from MAVProxy.modules.mavproxy_joystick import controls
+
+
+class Joystick(mp_module.MPModule):
+    '''
+    joystick set verbose
+    joystick set debug
+    joystick status
+    joystick probe
+    '''
+
+    def __init__(self, mpstate):
+        """Initialise module"""
+        super(Joystick, self).__init__(mpstate, 'joystick',
+                                       'A flexible joystick driver')
+
+        self.joystick = None
+
+        self.init_pygame()
+        self.init_settings()
+        self.init_commands()
+
+        self.probe()
+
+    def log(self, msg, level=0):
+        if self.mpstate.settings.moddebug < level:
+            return
+
+        print('{}: {}'.format(__name__, msg))
+
+    def init_pygame(self):
+        self.log('Initializing pygame', 2)
+        pygame.init()
+        pygame.joystick.init()
+
+    def init_settings(self):
+        pass
+
+    def init_commands(self):
+        self.log('Initializing commands', 2)
+        self.add_command('joystick', self.cmd_joystick,
+                         "A flexible joystick drvier",
+                         ['status',  'probe'])
+
+    def load_definitions(self):
+        self.log('Loading joystick definitions', 1)
+
+        self.joydefs = []
+        search = []
+
+        userjoysticks = os.environ.get(
+            'MAVPROXY_JOYSTICK_DIR',
+            mp_util.dot_mavproxy('joysticks'))
+        if userjoysticks is not None and os.path.isdir(userjoysticks):
+            search.append(userjoysticks)
+
+        search.append(pkg_resources.resource_filename(__name__, 'joysticks'))
+
+        for path in search:
+            self.log('Looking for joystick definitions in {}'.format(path),
+                     2)
+            path = os.path.expanduser(path)
+            for dirpath, dirnames, filenames in os.walk(path):
+                for joyfile in filenames:
+                    root, ext = os.path.splitext(joyfile)
+                    if ext[1:] not in ['yml', 'yaml', 'json']:
+                        continue
+
+                    joypath = os.path.join(dirpath, joyfile)
+                    self.log('Loading definition from {}'.format(joypath), 2)
+                    with open(joypath, 'r') as fd:
+                        joydef = yaml.safe_load(fd)
+                        joydef['path'] = joypath
+                        self.joydefs.append(joydef)
+
+    def probe(self):
+        self.load_definitions()
+
+        for jid in range(pygame.joystick.get_count()):
+            joy = pygame.joystick.Joystick(jid)
+            for joydef in self.joydefs:
+                if 'match' not in joydef:
+                    self.log('{} has no match patterns, ignoring.'.format(
+                        joydef['path']), 0)
+                    continue
+                for pattern in joydef['match']:
+                    if fnmatch.fnmatch(joy.get_name().lower(),
+                                       pattern.lower()):
+                        self.log('Using {} ("{}" matches pattern "{}")'.format(
+                            joydef['path'], joy.get_name(), pattern))
+                        self.joystick = controls.Joystick(joy, joydef)
+                        return
+
+        print('{}: Failed to find matching joystick.'.format(__name__))
+
+    def usage(self):
+        '''show help on command line options'''
+        return "Usage: joystick <status|set>"
+
+    def cmd_joystick(self, args):
+        if not len(args):
+            self.log('No subcommand specified.')
+        elif args[0] == 'status':
+            self.cmd_status()
+        elif args[0] == 'probe':
+            self.cmd_probe()
+        elif args[0] == 'help':
+            self.cmd_help()
+
+    def cmd_help(self):
+        print('joystick probe -- reload and match joystick definitions')
+        print('joystick status -- show currently loaded definition, if any')
+
+    def cmd_probe(self):
+        self.log('Re-detecting available joysticks', 0)
+        self.probe()
+
+    def cmd_status(self):
+        if self.joystick is None:
+            print('No active joystick')
+        else:
+            print('Active joystick:')
+            print('Path: {path}'.format(**self.joystick.controls))
+            print('Description: {description}'.format(
+                **self.joystick.controls))
+
+    def idle_task(self):
+        if self.joystick is None:
+            return
+
+        for e in pygame.event.get():
+            override = self.module('rc').override[:]
+            values = self.joystick.read()
+            override = values + override[len(values):]
+
+            self.log('channels: {}'.format(override), level=3)
+
+            if override != self.module('rc').override:
+                self.module('rc').override = override
+                self.module('rc').override_period.force()
+
+
+def init(mpstate):
+    '''initialise module'''
+    return Joystick(mpstate)

MAVProxy/modules/mavproxy_joystick/controls.py

@@ -0,0 +1,156 @@
+'''Joystick control classes'''
+
+
+def scale(val,
+          inlow=-1, inhigh=1,
+          outlow=1000, outhigh=2000):
+    '''Scale an in value in the range (inlow, inhigh) to the
+    range (outlow, outhigh).'''
+    return (
+        ((float(val) - inlow) / (inhigh - inlow)) *
+        (outhigh - outlow) + outlow
+    )
+
+
+class Control (object):
+    '''Base class for all controls'''
+    def __init__(self, joystick,
+                 inlow=-1, inhigh=1,
+                 outlow=1000, outhigh=2000):
+        self.joystick = joystick
+        self.inlow = inlow
+        self.inhigh = inhigh
+        self.outlow = outlow
+        self.outhigh = outhigh
+
+
+class Button (Control):
+    '''A Button acts like a momentary switch.  When pressed, the
+    corresponding channel value is set to `outhigh`; when released,
+    the value is set to `outlow`.'''
+
+    def __init__(self, joystick, id, **kwargs):
+        super(Button, self).__init__(joystick, **kwargs)
+        self.id = id
+
+    @property
+    def value(self):
+        state = self.joystick.get_button(self.id)
+        if state:
+            return self.outhigh
+        else:
+            return self.outlow
+
+
+class MultiButton (Control):
+    '''A MultiButton maps multiple buttons to the same channel like a
+    multiple-position switch.  When a button is pressed, the channel
+    is set to the corresponding value.  When a button is released, no
+    changes is made to the channel.'''
+
+    def __init__(self, joystick, buttons, **kwargs):
+        super(MultiButton, self).__init__(joystick, **kwargs)
+        self.buttons = buttons
+        self._value = buttons[0]['value']
+
+    @property
+    def value(self):
+        for button in self.buttons:
+            state = self.joystick.get_button(button['id'])
+            if state:
+                self._value = button['value']
+                break
+
+        return self._value
+
+
+class Axis (Control):
+    '''An Axis maps a joystick axis to a channel.  Set `invert` to
+    `True` in order to reverse the direction of the input.'''
+
+    def __init__(self, joystick, id, invert=False, **kwargs):
+        super(Axis, self).__init__(joystick, **kwargs)
+        self.id = id
+        self.invert = invert
+
+    @property
+    def value(self):
+        val = self.joystick.get_axis(self.id)
+        if self.invert:
+            val = -val
+
+        return scale(val, outlow=self.outlow, outhigh=self.outhigh)
+
+
+class Hat (Control):
+    '''A Hat maps one axis of a hat as if it were a toggle switch.
+    When the axis goes negative, the corresponding channel value is
+    set to `outputlow`.  When the axis goes positive, the value is set
+    to `outputhigh`.  No change is made when the axis returns to 0.'''
+
+    def __init__(self, joystick, id, axis, **kwargs):
+        super(Hat, self).__init__(joystick, **kwargs)
+        self.id = id
+        self.axis = axis
+        self._value = self.outlow
+
+    @property
+    def value(self):
+        x, y = self.joystick.get_hat(self.id)
+
+        value = x if self.axis == 'x' else y
+
+        if value != 0:
+            self._value = scale(value,
+                                outlow=self.outlow, outhigh=self.outhigh)
+
+        return self._value
+
+
+class Joystick (object):
+    '''A Joystick manages a collection of Controls.'''
+
+    def __init__(self, joystick, controls):
+        self.joystick = joystick
+        self.controls = controls
+
+        self.chan_max = max(control['channel']
+                            for control in controls['controls'])
+        self.channels = [None] * self.chan_max
+
+        self.joystick.init()
+
+        for control in controls['controls']:
+            if control['type'] == 'button':
+                kwargs = {k: control[k]
+                          for k in control.keys()
+                          if k in ['outlow', 'outhigh']}
+
+                handler = Button(self.joystick, control['id'], **kwargs)
+
+            elif control['type'] == 'axis':
+                kwargs = {k: control[k]
+                          for k in control.keys()
+                          if k in ['inlow', 'inhigh',
+                                   'outlow', 'outhigh', 'invert']}
+
+                handler = Axis(self.joystick, control['id'], **kwargs)
+
+            elif control['type'] == 'multibutton':
+                handler = MultiButton(self.joystick,
+                                   buttons=control['buttons'])
+            elif control['type'] == 'hat':
+                kwargs = {k: control[k]
+                          for k in control.keys()
+                          if k in ['outlow', 'outhigh']}
+
+                handler = Hat(self.joystick, control['id'], control['axis'])
+
+            self.channels[control['channel']-1] = handler
+
+    def read(self):
+        '''Returns an array of channel values.  Return 0 for channels
+        not specified in the control definition.'''
+
+        return [int(handler.value) if handler is not None else 0
+                for handler in self.channels]