Browse Source
That is added as a reference implementation on how to interpret the field `completion_mask` from the `MAG_CAL_PROGRESS` mavlink message.master
Gustavo Jose de Sousa
9 years ago
committed by
Lucas De Marchi
5 changed files with 434 additions and 0 deletions
@ -0,0 +1,64 @@
@@ -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 @@
@@ -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 @@
@@ -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