diff --git a/Tools/mavproxy_modules/README.md b/Tools/mavproxy_modules/README.md
index 5db2ef7a32..869f944ddd 100644
--- a/Tools/mavproxy_modules/README.md
+++ b/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.
diff --git a/Tools/mavproxy_modules/lib/__init__.py b/Tools/mavproxy_modules/lib/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/Tools/mavproxy_modules/lib/geodesic_grid.py b/Tools/mavproxy_modules/lib/geodesic_grid.py
new file mode 100644
index 0000000000..060bd11b37
--- /dev/null
+++ b/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 .
+'''
+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),
+ )
diff --git a/Tools/mavproxy_modules/lib/magcal_graph_ui.py b/Tools/mavproxy_modules/lib/magcal_graph_ui.py
new file mode 100644
index 0000000000..cdee193353
--- /dev/null
+++ b/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 .
+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: 'Success',
+ mavlink.MAG_CAL_FAILED: 'Failed',
+ }
+
+ _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(
+ 'Status: %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(
+ 'Compass id: %d' % m.compass_id
+ )
+
+ self._completion_pct_text.SetLabelMarkup(
+ 'Completion: %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('No calibration messages received yet...')
+ 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)
diff --git a/Tools/mavproxy_modules/magcal_graph.py b/Tools/mavproxy_modules/magcal_graph.py
new file mode 100644
index 0000000000..ec034bf6fd
--- /dev/null
+++ b/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 .
+'''
+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)