Mag[i]cal graph

Tools/autotest/sim_vehicle.py

@@ -8,6 +8,7 @@
 import sys
 import atexit
 import os
+import os.path
 import subprocess
 import tempfile
 import getpass
@@ -271,6 +272,9 @@ def find_root_dir():
 extra_mavlink_cmds: extra parameters that will be passed to mavproxy
 '''
 _options_for_frame = {
+    "calibration": {
+        "extra_mavlink_cmds": "module load sitl_calibration;",
+    },
     "+": {
         "waf_target": "bin/arducopter-quad",
         "default_params_filename": "copter_params.parm"
@@ -496,9 +500,9 @@ def progress_cmd(what, cmd):
     shell_text = "%s" % (" ".join([ '"%s"' % x for x in cmd ]))
     progress(shell_text)
 
-def run_cmd_blocking(what, cmd):
+def run_cmd_blocking(what, cmd, **kw):
     progress_cmd(what, cmd)
-    p = subprocess.Popen(cmd)
+    p = subprocess.Popen(cmd, **kw)
     return os.waitpid(p.pid,0)
 
 def run_in_terminal_window(autotest, name, cmd):
@@ -618,7 +622,12 @@ def start_mavproxy(opts, stuff):
     if len(extra_cmd):
         cmd.extend(['--cmd', extra_cmd])
 
-    run_cmd_blocking("Run MavProxy", cmd)
+    local_mp_modules_dir = os.path.abspath(
+            os.path.join(__file__, '..', '..', 'mavproxy_modules'))
+    env = dict(os.environ)
+    env['PYTHONPATH'] = local_mp_modules_dir + os.pathsep + env.get('PYTHONPATH', '')
+
+    run_cmd_blocking("Run MavProxy", cmd, env=env)
     progress("MAVProxy exitted")
 
 frame_options = options_for_frame(opts.frame, opts.vehicle, opts)

Tools/mavproxy_modules/README.md

@@ -10,6 +10,10 @@ This module interfaces with the `calibration` model of SITL. It provides
 commands to actuate on the vehicle's rotation to simulate a calibration
 process.
 
+Make sure to pass `--model calibration` to the SITL binary in order to be able
+use this module's commands. You can also use
+`[sim_vehicle.py](../autotest/sim_vehicle.py)` with `--frame calibration`.
+
 ### Accelerometer Calibration ###
 The command `sitl_accelcal` listens to the accelerometer calibration status
 texts and set the vehicle in the desired attitude. Example:
@@ -31,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)