Tools: magcal_graph: add mavproxy module

That is added as a reference implementation on how to interpret the field
`completion_mask` from the `MAG_CAL_PROGRESS` mavlink message.

Tools/mavproxy_modules/README.md

@@ -35,3 +35,17 @@ There are other commands you can use with this module:
  - `sitl_attitude`: set vehicle at a desired attitude
  - `sitl_angvel`: apply angular velocity on the vehicle
  - `sitl_stop`: stop any of this module's currently active command
+
+## `magcal_graph` ##
+This module shows the geodesic sections hit by the samples collected during
+compass calibration, and also some status data. The objective of this module is
+to provide a reference on how to interpret the field `completion_mask` from the
+`MAG_CAL_PROGRESS` mavlink message. That information can be used in order to
+guide the vehicle user during calibration.
+
+The plot shown by this module isn't very helpful to the end user, but it might
+help developers during development of internal calibration support in ground
+control stations.
+
+The only command provided by this module is `magcal_graph`, which will open the
+graphical user interface.

Tools/mavproxy_modules/lib/geodesic_grid.py

@@ -0,0 +1,64 @@
+# Copyright (C) 2016  Intel Corporation. All rights reserved.
+#
+# This file is free software: you can redistribute it and/or modify it
+# under the terms of the GNU General Public License as published by the
+# Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This file is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program.  If not, see <http://www.gnu.org/licenses/>.
+'''
+This module takes libraries/AP_Math/AP_GeodesicGrid.h reference for defining
+the geodesic sections.
+'''
+import math
+from scipy.constants import golden as g
+
+_first_half = (
+    ((-g, 1, 0), (-1, 0,-g), (-g,-1, 0)),
+    ((-1, 0,-g), (-g,-1, 0), ( 0,-g,-1)),
+    ((-g,-1, 0), ( 0,-g,-1), ( 0,-g, 1)),
+    ((-1, 0,-g), ( 0,-g,-1), ( 1, 0,-g)),
+    (( 0,-g,-1), ( 0,-g, 1), ( g,-1, 0)),
+    (( 0,-g,-1), ( 1, 0,-g), ( g,-1, 0)),
+    (( g,-1, 0), ( 1, 0,-g), ( g, 1, 0)),
+    (( 1, 0,-g), ( g, 1, 0), ( 0, g,-1)),
+    (( 1, 0,-g), ( 0, g,-1), (-1, 0,-g)),
+    (( 0, g,-1), (-g, 1, 0), (-1, 0,-g)),
+)
+_second_half = tuple(
+    ((-xa, -ya, -za), (-xb, -yb, -zb), (-xc, -yc, -zc))
+    for (xa, ya, za), (xb, yb, zb), (xc, yc, zc) in _first_half
+)
+
+triangles = _first_half + _second_half
+
+def _midpoint_projection(a, b):
+    xa, ya, za = a
+    xb, yb, zb = b
+    s = _midpoint_projection.scale
+    return s * (xa + xb), s * (ya + yb), s * (za + zb)
+
+radius = math.sqrt(1 + g**2)
+
+# radius / (length of two vertices of an icosahedron triangle)
+_midpoint_projection.scale = radius / (2 * g)
+
+sections_triangles = ()
+
+for a, b, c in triangles:
+    ma = _midpoint_projection(a, b)
+    mb = _midpoint_projection(b, c)
+    mc = _midpoint_projection(c, a)
+
+    sections_triangles += (
+        (ma, mb, mc),
+        ( a, ma, mc),
+        (ma,  b, mb),
+        (mc, mb,  c),
+    )

Tools/mavproxy_modules/lib/magcal_graph_ui.py

@@ -0,0 +1,246 @@
+# Copyright (C) 2016  Intel Corporation. All rights reserved.
+#
+# This file is free software: you can redistribute it and/or modify it
+# under the terms of the GNU General Public License as published by the
+# Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This file is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program.  If not, see <http://www.gnu.org/licenses/>.
+import matplotlib.pyplot as plt
+from matplotlib.backends.backend_wxagg import FigureCanvas
+from mpl_toolkits.mplot3d import Axes3D
+from mpl_toolkits.mplot3d.art3d import Poly3DCollection
+from pymavlink.mavutil import mavlink
+
+from MAVProxy.modules.lib import wx_processguard
+from MAVProxy.modules.lib.wx_loader import wx
+
+import geodesic_grid as grid
+
+class MagcalPanel(wx.Panel):
+    _status_markup_strings = {
+        mavlink.MAG_CAL_NOT_STARTED: 'Not started',
+        mavlink.MAG_CAL_WAITING_TO_START: 'Waiting to start',
+        mavlink.MAG_CAL_RUNNING_STEP_ONE: 'Step one',
+        mavlink.MAG_CAL_RUNNING_STEP_TWO: 'Step two',
+        mavlink.MAG_CAL_SUCCESS: '<span color="blue">Success</span>',
+        mavlink.MAG_CAL_FAILED: '<span  color="red">Failed</span>',
+    }
+
+    _empty_color = '#7ea6ce'
+    _filled_color = '#4680b9'
+
+    def __init__(self, *k, **kw):
+        super(MagcalPanel, self).__init__(*k, **kw)
+
+        facecolor = self.GetBackgroundColour().GetAsString(wx.C2S_HTML_SYNTAX)
+        fig = plt.figure(facecolor=facecolor, figsize=(1,1))
+
+        self._canvas = FigureCanvas(self, wx.ID_ANY, fig)
+        self._canvas.SetMinSize((300,300))
+
+        self._id_text = wx.StaticText(self, wx.ID_ANY)
+        self._status_text = wx.StaticText(self, wx.ID_ANY)
+        self._completion_pct_text = wx.StaticText(self, wx.ID_ANY)
+
+        sizer = wx.BoxSizer(wx.VERTICAL)
+        sizer.Add(self._id_text)
+        sizer.Add(self._status_text)
+        sizer.Add(self._completion_pct_text)
+        sizer.Add(self._canvas, proportion=1, flag=wx.EXPAND)
+        self.SetSizer(sizer)
+
+        ax = fig.add_subplot(111, axis_bgcolor=facecolor, projection='3d')
+        self.configure_plot(ax)
+
+    def configure_plot(self, ax):
+        extra = .5
+        lim = grid.radius + extra
+        ax.set_xlim3d(-lim, lim)
+        ax.set_ylim3d(-lim, lim)
+        ax.set_zlim3d(-lim, lim)
+
+        ax.set_xlabel('x')
+        ax.set_ylabel('y')
+        ax.set_zlabel('z')
+
+        ax.invert_zaxis()
+        ax.invert_xaxis()
+
+        ax.set_aspect('equal')
+
+        self._polygons_collection = Poly3DCollection(
+            grid.sections_triangles,
+            edgecolors='#386694',
+        )
+        ax.add_collection3d(self._polygons_collection)
+
+    def update_status_from_mavlink(self, m):
+        status_string = self._status_markup_strings.get(m.cal_status, '???')
+        self._status_text.SetLabelMarkup(
+            '<b>Status:</b> %s' % status_string,
+        )
+
+    def mavlink_magcal_report(self, m):
+        self.update_status_from_mavlink(m)
+        self._completion_pct_text.SetLabel('')
+
+    def mavlink_magcal_progress(self, m):
+        facecolors = []
+        for i, mask in enumerate(m.completion_mask):
+            for j in range(8):
+                section = i * 8 + j
+                if mask & 1 << j:
+                    facecolor = self._filled_color
+                else:
+                    facecolor = self._empty_color
+                facecolors.append(facecolor)
+        self._polygons_collection.set_facecolors(facecolors)
+        self._canvas.draw()
+
+        self._id_text.SetLabelMarkup(
+            '<b>Compass id:</b> %d' % m.compass_id
+        )
+
+        self._completion_pct_text.SetLabelMarkup(
+            '<b>Completion:</b> %d%%' % m.completion_pct
+        )
+
+        self.update_status_from_mavlink(m)
+
+    _legend_panel = None
+    @staticmethod
+    def legend_panel(*k, **kw):
+        if MagcalPanel._legend_panel:
+            return MagcalPanel._legend_panel
+
+        p = MagcalPanel._legend_panel = wx.Panel(*k, **kw)
+        sizer = wx.BoxSizer(wx.HORIZONTAL)
+        p.SetSizer(sizer)
+
+        marker = wx.Panel(p, wx.ID_ANY, size=(10, 10))
+        marker.SetBackgroundColour(MagcalPanel._empty_color)
+        sizer.Add(marker, flag=wx.ALIGN_CENTER)
+        text = wx.StaticText(p, wx.ID_ANY)
+        text.SetLabel('Sections not hit')
+        sizer.Add(text, border=4, flag=wx.ALIGN_CENTER | wx.LEFT)
+
+        marker = wx.Panel(p, wx.ID_ANY, size=(10, 10))
+        marker.SetBackgroundColour(MagcalPanel._filled_color)
+        sizer.Add(marker, border=10, flag=wx.ALIGN_CENTER | wx.LEFT)
+        text = wx.StaticText(p, wx.ID_ANY)
+        text.SetLabel('Sections hit')
+        sizer.Add(text, border=4, flag=wx.ALIGN_CENTER | wx.LEFT)
+        return p
+
+class MagcalFrame(wx.Frame):
+    def __init__(self, conn):
+        super(MagcalFrame, self).__init__(
+            None,
+            wx.ID_ANY,
+            title='Magcal Graph',
+        )
+
+        self.SetMinSize((300, 300))
+
+        self._conn = conn
+
+        self._main_panel = wx.ScrolledWindow(self, wx.ID_ANY)
+        self._main_panel.SetScrollbars(1, 1, 1, 1)
+
+        self._magcal_panels = {}
+
+        self._sizer = wx.BoxSizer(wx.VERTICAL)
+        self._main_panel.SetSizer(self._sizer)
+
+        idle_text = wx.StaticText(self._main_panel, wx.ID_ANY)
+        idle_text.SetLabelMarkup('<i>No calibration messages received yet...</i>')
+        idle_text.SetForegroundColour('#444444')
+
+        self._sizer.AddStretchSpacer()
+        self._sizer.Add(
+            idle_text,
+            proportion=0,
+            flag=wx.ALIGN_CENTER | wx.ALL,
+            border=10,
+        )
+        self._sizer.AddStretchSpacer()
+
+        self._timer = wx.Timer(self)
+        self.Bind(wx.EVT_TIMER, self.timer_callback, self._timer)
+        self._timer.Start(200)
+
+    def add_compass(self, id):
+        if not self._magcal_panels:
+            self._sizer.Clear(deleteWindows=True)
+            self._magcal_panels_sizer = wx.BoxSizer(wx.HORIZONTAL)
+
+            self._sizer.Add(
+                self._magcal_panels_sizer,
+                proportion=1,
+                flag=wx.EXPAND,
+            )
+
+            legend = MagcalPanel.legend_panel(self._main_panel, wx.ID_ANY)
+            self._sizer.Add(
+                legend,
+                proportion=0,
+                flag=wx.ALIGN_CENTER,
+            )
+
+        self._magcal_panels[id] = MagcalPanel(self._main_panel, wx.ID_ANY)
+        self._magcal_panels_sizer.Add(
+            self._magcal_panels[id],
+            proportion=1,
+            border=10,
+            flag=wx.EXPAND | wx.ALL,
+        )
+
+    def timer_callback(self, evt):
+        close_requested = False
+        mavlink_msgs = {}
+        while self._conn.poll():
+            m = self._conn.recv()
+            if isinstance(m, str) and m == 'close':
+                close_requested = True
+                continue
+            if m.compass_id not in mavlink_msgs:
+                # Keep the last two messages so that we get the last progress
+                # if the last message is the calibration report.
+                mavlink_msgs[m.compass_id] = [None, m]
+            else:
+                l = mavlink_msgs[m.compass_id]
+                l[0] = l[1]
+                l[1] = m
+
+        if close_requested:
+            self._timer.Stop()
+            self.Destroy()
+            return
+
+        if not mavlink_msgs:
+            return
+
+        needs_fit = False
+        for k in mavlink_msgs:
+            if k not in self._magcal_panels:
+                self.add_compass(k)
+                needs_fit = True
+        if needs_fit:
+            self._sizer.Fit(self)
+
+        for k, l in mavlink_msgs.items():
+            for m in l:
+                if not m:
+                    continue
+                panel = self._magcal_panels[k]
+                if m.get_type() == 'MAG_CAL_PROGRESS':
+                    panel.mavlink_magcal_progress(m)
+                elif m.get_type() == 'MAG_CAL_REPORT':
+                    panel.mavlink_magcal_report(m)

Tools/mavproxy_modules/magcal_graph.py

@@ -0,0 +1,110 @@
+# Copyright (C) 2016  Intel Corporation. All rights reserved.
+#
+# This file is free software: you can redistribute it and/or modify it
+# under the terms of the GNU General Public License as published by the
+# Free Software Foundation, either version 3 of the License, or
+# (at your option) any later version.
+#
+# This file is distributed in the hope that it will be useful, but
+# WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+# See the GNU General Public License for more details.
+#
+# You should have received a copy of the GNU General Public License along
+# with this program.  If not, see <http://www.gnu.org/licenses/>.
+'''
+This module shows the geodesic sections hit by the samples collected during
+compass calibration, and also some status data. The objective of this module is
+to provide a reference on how to interpret the field `completion_mask` from the
+MAG_CAL_PROGRESS mavlink message. That information can be used in order to
+guide the vehicle user during calibration.
+
+The plot shown by this module isn't very helpful to the end user, but it might
+help developers during development of internal calibration support in ground
+control stations.
+'''
+from MAVProxy.modules.lib import mp_module, mp_util
+import multiprocessing
+
+class MagcalGraph():
+    def __init__(self):
+        self.parent_pipe, self.child_pipe = multiprocessing.Pipe()
+        self.ui_process = None
+        self._last_mavlink_msgs = {}
+
+    def start(self):
+        if self.is_active():
+            return
+        if self.ui_process:
+            self.ui_process.join()
+
+        for l in self._last_mavlink_msgs.values():
+            for m in l:
+                if not m:
+                    continue
+                self.parent_pipe.send(m)
+
+        self.ui_process = multiprocessing.Process(target=self.ui_task)
+        self.ui_process.start()
+
+    def stop(self):
+        if not self.is_active():
+            return
+
+        self.parent_pipe.send('close')
+        self.ui_process.join()
+
+    def ui_task(self):
+        mp_util.child_close_fds()
+
+        from MAVProxy.modules.lib import wx_processguard
+        from MAVProxy.modules.lib.wx_loader import wx
+        from lib.magcal_graph_ui import MagcalFrame
+
+        app = wx.App(False)
+        app.frame = MagcalFrame(self.child_pipe)
+        app.frame.Show()
+        app.MainLoop()
+
+    def is_active(self):
+        return self.ui_process is not None and self.ui_process.is_alive()
+
+    def mavlink_packet(self, m):
+        if m.compass_id not in self._last_mavlink_msgs:
+            # Keep the two last messages so that, if one is the calibration
+            # report message, the previous one is the last progress message.
+            self._last_mavlink_msgs[m.compass_id] = [None, m]
+        else:
+            l = self._last_mavlink_msgs[m.compass_id]
+            l[0] = l[1]
+            l[1] = m
+
+        if not self.is_active():
+            return
+        self.parent_pipe.send(m)
+
+class MagcalGraphModule(mp_module.MPModule):
+    def __init__(self, mpstate):
+        super(MagcalGraphModule, self).__init__(mpstate, 'magcal_graph')
+        self.add_command(
+            'magcal_graph',
+            self.cmd_magcal_graph,
+            'open a window to report magcal progress and plot geodesic ' +
+            'sections hit by the collected data in real time',
+        )
+
+        self.graph = MagcalGraph()
+
+    def cmd_magcal_graph(self, args):
+        self.graph.start()
+
+    def mavlink_packet(self, m):
+        if m.get_type() not in ('MAG_CAL_PROGRESS', 'MAG_CAL_REPORT'):
+            return
+        self.graph.mavlink_packet(m)
+
+    def unload(self):
+        self.graph.stop()
+
+def init(mpstate):
+    return MagcalGraphModule(mpstate)