Mag viewer (#3465)

* [supervision] Add raw mag viewer tool

* [supervision] mag viewer improvements

* [supervision] organize utilities with tabs

* [supervision] MagViewer: do not normalize data, and adjust view size automatically

* [supervision] MagViewer: prepare calibration

---------

Co-authored-by: Fabien-B <Fabien-B@github.com>

sw/supervision/python/generated/ui_supervision_window.py

@@ -2,9 +2,10 @@
 
 # Form implementation generated from reading ui file 'ui/ui_supervision_window.ui'
 #
-# Created by: PyQt5 UI code generator 5.14.1
+# Created by: PyQt5 UI code generator 5.15.10
 #
-# WARNING! All changes made in this file will be lost!
+# WARNING: Any manual changes made to this file will be lost when pyuic5 is
+# run again.  Do not edit this file unless you know what you are doing.
 
 
 from PyQt5 import QtCore, QtGui, QtWidgets
@@ -32,29 +33,31 @@ class Ui_SupervisionWindow(object):
         self.doc_panel = DocPanel()
         self.doc_panel.setObjectName("doc_panel")
         self.tabwidget.addTab(self.doc_panel, "")
-        self.tools_panel = QtWidgets.QWidget()
-        self.tools_panel.setObjectName("tools_panel")
-        self.gridLayout = QtWidgets.QGridLayout(self.tools_panel)
-        self.gridLayout.setObjectName("gridLayout")
-        self.groupBox = QtWidgets.QGroupBox(self.tools_panel)
-        self.groupBox.setObjectName("groupBox")
-        self.verticalLayout_2 = QtWidgets.QVBoxLayout(self.groupBox)
-        self.verticalLayout_2.setObjectName("verticalLayout_2")
-        self.log_widget = LogWidget(self.groupBox)
+        self.tab = QtWidgets.QWidget()
+        self.tab.setObjectName("tab")
+        self.verticalLayout_4 = QtWidgets.QVBoxLayout(self.tab)
+        self.verticalLayout_4.setObjectName("verticalLayout_4")
+        self.tabWidget = QtWidgets.QTabWidget(self.tab)
+        self.tabWidget.setObjectName("tabWidget")
+        self.tab_log = QtWidgets.QWidget()
+        self.tab_log.setObjectName("tab_log")
+        self.verticalLayout_5 = QtWidgets.QVBoxLayout(self.tab_log)
+        self.verticalLayout_5.setObjectName("verticalLayout_5")
+        self.log_widget = LogWidget(self.tab_log)
         self.log_widget.setObjectName("log_widget")
-        self.verticalLayout_2.addWidget(self.log_widget)
-        self.gridLayout.addWidget(self.groupBox, 0, 0, 1, 1)
-        spacerItem = QtWidgets.QSpacerItem(40, 20, QtWidgets.QSizePolicy.Expanding, QtWidgets.QSizePolicy.Minimum)
-        self.gridLayout.addItem(spacerItem, 0, 1, 1, 1)
-        spacerItem1 = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding)
-        self.gridLayout.addItem(spacerItem1, 1, 0, 1, 1)
-        self.gridLayout.setColumnStretch(0, 2)
-        self.gridLayout.setColumnStretch(1, 1)
-        self.tabwidget.addTab(self.tools_panel, "")
+        self.verticalLayout_5.addWidget(self.log_widget)
+        spacerItem = QtWidgets.QSpacerItem(20, 40, QtWidgets.QSizePolicy.Minimum, QtWidgets.QSizePolicy.Expanding)
+        self.verticalLayout_5.addItem(spacerItem)
+        self.tabWidget.addTab(self.tab_log, "")
+        self.magviewer_widget = MagViewer()
+        self.magviewer_widget.setObjectName("magviewer_widget")
+        self.tabWidget.addTab(self.magviewer_widget, "")
+        self.verticalLayout_4.addWidget(self.tabWidget)
+        self.tabwidget.addTab(self.tab, "")
         self.verticalLayout.addWidget(self.tabwidget)
         SupervisionWindow.setCentralWidget(self.centralwidget)
         self.menubar = QtWidgets.QMenuBar(SupervisionWindow)
-        self.menubar.setGeometry(QtCore.QRect(0, 0, 800, 22))
+        self.menubar.setGeometry(QtCore.QRect(0, 0, 800, 30))
         self.menubar.setObjectName("menubar")
         self.menuFile = QtWidgets.QMenu(self.menubar)
         self.menuFile.setObjectName("menuFile")
@@ -75,6 +78,7 @@ class Ui_SupervisionWindow(object):
 
         self.retranslateUi(SupervisionWindow)
         self.tabwidget.setCurrentIndex(0)
+        self.tabWidget.setCurrentIndex(0)
         QtCore.QMetaObject.connectSlotsByName(SupervisionWindow)
 
     def retranslateUi(self, SupervisionWindow):
@@ -83,8 +87,9 @@ class Ui_SupervisionWindow(object):
         self.tabwidget.setTabText(self.tabwidget.indexOf(self.configuration_panel), _translate("SupervisionWindow", "Configuration"))
         self.tabwidget.setTabText(self.tabwidget.indexOf(self.operation_panel), _translate("SupervisionWindow", "Operation"))
         self.tabwidget.setTabText(self.tabwidget.indexOf(self.doc_panel), _translate("SupervisionWindow", "Documentation"))
-        self.groupBox.setTitle(_translate("SupervisionWindow", "Extract SD logs"))
-        self.tabwidget.setTabText(self.tabwidget.indexOf(self.tools_panel), _translate("SupervisionWindow", "Utilities"))
+        self.tabWidget.setTabText(self.tabWidget.indexOf(self.tab_log), _translate("SupervisionWindow", "SD log"))
+        self.tabWidget.setTabText(self.tabWidget.indexOf(self.magviewer_widget), _translate("SupervisionWindow", "Mag Viewer"))
+        self.tabwidget.setTabText(self.tabwidget.indexOf(self.tab), _translate("SupervisionWindow", "Utilities"))
         self.menuFile.setTitle(_translate("SupervisionWindow", "File"))
         self.menuHelp.setTitle(_translate("SupervisionWindow", "Help"))
         self.settings_action.setText(_translate("SupervisionWindow", "Edit Settings"))
@@ -93,4 +98,5 @@ from configuration_panel import ConfigurationPanel
 from doc_panel import DocPanel
 from header_widget import HeaderWidget
 from log_widget import LogWidget
+from mag_viewer import MagViewer
 from operation_panel import OperationPanel

sw/supervision/python/mag_viewer.py

@@ -0,0 +1,192 @@
+import sys
+import numpy as np
+from PyQt5 import QtWidgets, QtCore
+import utils
+from PyQt5.QtWidgets import QSizePolicy as QSP
+
+try:
+    import pyqtgraph as pg
+    import pyqtgraph.opengl as gl
+
+    #raise ModuleNotFoundError()
+
+    sys.path.append(utils.PAPARAZZI_SRC + "/sw/lib/python")
+    sys.path.append(utils.PAPARAZZI_HOME + "/var/lib/python") # pprzlink
+
+    from pprzlink.message import PprzMessage
+    from pprz_connect import PprzConnect, PprzConfig
+
+    class MagViewer(QtWidgets.QWidget):
+
+        mag_sig = QtCore.pyqtSignal(tuple)
+
+        def __init__(self, parent=None):
+            super().__init__(parent)
+
+            self.mag_data = np.empty((0,3))
+
+            self.vlay = QtWidgets.QVBoxLayout(self)
+
+            self.cmds_lay = QtWidgets.QHBoxLayout()
+            self.vlay.addLayout(self.cmds_lay)
+            
+            self.setup_cmds()
+
+            self.gl_widget = gl.GLViewWidget()
+            self.vlay.addWidget(self.gl_widget)
+            self.gl_widget.setCameraPosition(distance=20)
+
+            # Example axis
+            self.axis = gl.GLAxisItem()
+            self.axis.setSize(2, 2, 2)
+            self.gl_widget.addItem(self.axis)
+            # Axis Texts
+            self.txt_x = gl.GLTextItem(text='X', pos=[2, 0, 0])
+            self.gl_widget.addItem(self.txt_x)
+            self.txt_y = gl.GLTextItem(text='Y', pos=[0, 2, 0])
+            self.gl_widget.addItem(self.txt_y)
+            self.txt_z = gl.GLTextItem(text='Z', pos=[0, 0, 2])
+            self.gl_widget.addItem(self.txt_z)
+            # XY grid
+            self.xy_grid = gl.GLGridItem()
+            self.xy_grid.setSize(2, 2)
+            self.xy_grid.setSpacing(0.2, 0.2)
+            self.xy_grid.rotate(0, 1, 0, 0)
+            self.gl_widget.addItem(self.xy_grid)
+
+            shaft = np.array([[0,0,0], [1,0,0]])
+            self.arrow = gl.GLLinePlotItem(pos=shaft, width=2)
+            self.gl_widget.addItem(self.arrow)
+
+            # Placeholder for scatter plot
+            self.scatter = None
+            self.setMinimumSize(100, 100)
+            #self.gl_widget.setBackgroundColor((20, 20, 20, 255))
+
+            self.mean_norm = 1
+
+            self.connect = PprzConnect(notify=self.connect_cb)
+            self.mag_sig.connect(self.update_mag)
+
+            self.raw_bind_id = None
+            self.confs:dict[int, PprzConfig] = {}
+        
+        def setup_cmds(self):
+            mag_lbl = QtWidgets.QLabel("mags:", self)
+            mag_lbl.setSizePolicy(QSP.Policy.Fixed, QSP.Policy.Fixed)
+            self.cmds_lay.addWidget(mag_lbl)
+
+            self.mag_combo = QtWidgets.QComboBox(self)
+            self.cmds_lay.addWidget(self.mag_combo)
+            self.mag_combo.addItem("---")
+
+            self.ivy_sub_button = QtWidgets.QPushButton("Subscribe", self)
+            self.cmds_lay.addWidget(self.ivy_sub_button)
+            self.ivy_sub_button.clicked.connect(self.sub_unsub)
+
+            clear_button = QtWidgets.QPushButton("Clear data")
+            clear_button.setSizePolicy(QSP.Policy.Fixed, QSP.Policy.Fixed)
+            self.cmds_lay.addWidget(clear_button)
+            clear_button.clicked.connect(self.reset_data)
+
+            self.distance_auto_chk = QtWidgets.QCheckBox("size auto", self)
+            self.distance_auto_chk.setSizePolicy(QSP.Policy.Fixed, QSP.Policy.Fixed)
+            self.cmds_lay.addWidget(self.distance_auto_chk)
+            self.distance_auto_chk.setChecked(True)
+
+            calibrate_button = QtWidgets.QPushButton("calibrate", self)
+            calibrate_button.setSizePolicy(QSP.Policy.Fixed, QSP.Policy.Fixed)
+            self.cmds_lay.addWidget(calibrate_button)
+            calibrate_button.clicked.connect(self.calibrate)
+        
+        def stop(self):
+            self.connect.shutdown()
+        
+        def sub_unsub(self):
+            if self.raw_bind_id is None:
+                self.raw_bind_id = self.connect.ivy.subscribe(self.mag_raw_cb, PprzMessage("telemetry", "IMU_MAG_RAW"))
+                self.ivy_sub_button.setText("Unsubscribe")
+            else:
+                self.connect.ivy.unsubscribe(self.raw_bind_id)
+                self.ivy_sub_button.setText("Subscribe")
+                self.raw_bind_id = None
+
+        def reset_data(self):
+            self.mag_data = np.empty((0,3))
+            self.set_points(self.mag_data)
+            self.mean_norm = 1
+        
+        def connect_cb(self, conf: PprzConfig):
+            self.confs[conf.id] = conf
+
+        def mag_raw_cb(self, sender, msg):
+            ac_id = str(sender)
+            if ac_id not in self.confs:
+                return
+            ac_name = self.confs[ac_id].name
+            mag_id = f"{ac_name}: {msg['id']}"
+
+            mag_idx = self.mag_combo.findText(mag_id)
+            cur_idx = self.mag_combo.currentIndex()
+
+            if cur_idx == mag_idx:
+                mx = msg['mx']
+                my = msg['my']
+                mz = msg['mz']
+                mag = np.array([mx, my, mz])
+                self.mag_sig.emit((mag_id, mag))
+            elif mag_idx == -1:
+                self.mag_combo.addItem(mag_id)
+        
+        def update_mag(self, mag_data):
+            mid, mag = mag_data
+            self.mag_data = np.vstack([self.mag_data, mag])
+            mag_unit = mag / np.linalg.norm(mag)
+            self.set_points(self.mag_data)           
+            shaft = np.array([[0,0,0], mag_unit*self.mean_norm])
+            self.arrow.setData(pos=shaft)
+
+            if self.mag_data.shape[0] % 10 == 0:
+                self.resize_grid()
+        
+        def resize_grid(self):
+            norms = np.linalg.norm(self.mag_data, axis=1)
+            self.mean_norm = np.mean(norms)
+            if self.distance_auto_chk.isChecked():
+                self.txt_x.setData(pos=[self.mean_norm, 0, 0])
+                self.txt_y.setData(pos=[0, self.mean_norm, 0])
+                self.txt_z.setData(pos=[0, 0, self.mean_norm])
+                #print(mean_norm)
+                self.axis.setSize(self.mean_norm, self.mean_norm, self.mean_norm)
+                grid_size = 2*self.mean_norm
+                grid_spacing = self.mean_norm / 10
+                self.xy_grid.setSize(grid_size, grid_size, grid_size)
+                self.xy_grid.setSpacing(grid_spacing, grid_spacing, grid_spacing)
+                self.gl_widget.setCameraPosition(distance=self.mean_norm*5)
+        
+        def calibrate(self):
+            dia = QtWidgets.QMessageBox(QtWidgets.QMessageBox.Icon.Information,"Calibration", "plop\ntoto", QtWidgets.QMessageBox.StandardButton.Ok, self)
+            dia.open()
+
+
+        def set_points(self, points, color=(1, 0, 0, 1), size=5):
+            """
+            points: Nx3 numpy array
+            color: tuple (r, g, b, a)
+            size: point size
+            """
+            if self.scatter:
+                self.gl_widget.removeItem(self.scatter)
+            self.scatter = gl.GLScatterPlotItem(pos=points, color=color, size=size)
+            self.gl_widget.addItem(self.scatter)
+except ImportError:
+    
+    class MagViewer(QtWidgets.QWidget):
+        def __init__(self, parent=None):
+            super().__init__(parent)
+            self.vlay = QtWidgets.QVBoxLayout(self)
+            self.error_label = QtWidgets.QLabel("module not found!")
+            self.vlay.addWidget(self.error_label)
+        
+        def stop(self):
+            pass

sw/supervision/python/paparazzicenter.py

@@ -234,6 +234,7 @@ class PprzCenter(QMainWindow, Ui_SupervisionWindow):
         settings_dialog.show()
 
     def quit(self, interactive=True):
+        self.magviewer_widget.stop()
         quit_accepted = True
         if self.operation_panel.session.any_program_running():
             quit_accepted = False

sw/supervision/python/ui/ui_supervision_window.ui

@@ -38,49 +38,46 @@
         <string>Documentation</string>
        </attribute>
       </widget>
-      <widget class="QWidget" name="tools_panel">
+      <widget class="QWidget" name="tab">
        <attribute name="title">
         <string>Utilities</string>
        </attribute>
-       <layout class="QGridLayout" name="gridLayout" rowstretch="0,0" columnstretch="2,1">
-        <item row="0" column="0">
-         <widget class="QGroupBox" name="groupBox">
-          <property name="title">
-           <string>Extract SD logs</string>
+       <layout class="QVBoxLayout" name="verticalLayout_4">
+        <item>
+         <widget class="QTabWidget" name="tabWidget">
+          <property name="currentIndex">
+           <number>0</number>
           </property>
-          <layout class="QVBoxLayout" name="verticalLayout_2">
-           <item>
-            <widget class="LogWidget" name="log_widget" native="true"/>
-           </item>
-          </layout>
+          <widget class="QWidget" name="tab_log">
+           <attribute name="title">
+            <string>SD log</string>
+           </attribute>
+           <layout class="QVBoxLayout" name="verticalLayout_5">
+            <item>
+             <widget class="LogWidget" name="log_widget" native="true"/>
+            </item>
+            <item>
+             <spacer name="verticalSpacer">
+              <property name="orientation">
+               <enum>Qt::Vertical</enum>
+              </property>
+              <property name="sizeHint" stdset="0">
+               <size>
+                <width>20</width>
+                <height>40</height>
+               </size>
+              </property>
+             </spacer>
+            </item>
+           </layout>
+          </widget>
+          <widget class="MagViewer" name="magviewer_widget">
+           <attribute name="title">
+            <string>Mag Viewer</string>
+           </attribute>
+          </widget>
          </widget>
         </item>
-        <item row="0" column="1">
-         <spacer name="horizontalSpacer">
-          <property name="orientation">
-           <enum>Qt::Horizontal</enum>
-          </property>
-          <property name="sizeHint" stdset="0">
-           <size>
-            <width>40</width>
-            <height>20</height>
-           </size>
-          </property>
-         </spacer>
-        </item>
-        <item row="1" column="0">
-         <spacer name="verticalSpacer">
-          <property name="orientation">
-           <enum>Qt::Vertical</enum>
-          </property>
-          <property name="sizeHint" stdset="0">
-           <size>
-            <width>20</width>
-            <height>40</height>
-           </size>
-          </property>
-         </spacer>
-        </item>
        </layout>
       </widget>
      </widget>
@@ -93,7 +90,7 @@
      <x>0</x>
      <y>0</y>
      <width>800</width>
-     <height>22</height>
+     <height>30</height>
     </rect>
    </property>
    <widget class="QMenu" name="menuFile">
@@ -154,6 +151,12 @@
    <header>log_widget.h</header>
    <container>1</container>
   </customwidget>
+  <customwidget>
+   <class>MagViewer</class>
+   <extends>QWidget</extends>
+   <header>mag_viewer.h</header>
+   <container>1</container>
+  </customwidget>
  </customwidgets>
  <resources/>
  <connections/>