ardupilot/Tools/mavproxy_modules/lib/magcal_graph_ui.py

# 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)