mirror of https://github.com/ArduPilot/ardupilot
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.
This commit is contained in:
Gustavo Jose de Sousa
Lucas De Marchi
parent
2b57c146fe
commit
5b91a6e065
|
|
@ -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.
|
||||
|
|
|
|||
|
|
@ -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),
|
||||
)
|
||||
|
|
@ -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)
|
||||
|
|
@ -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)
|
||||
Loading…
Reference in New Issue