Rework the lighthouse geometry estimator

src/cfclient/ui/dialogs/lighthouse_bs_geometry_dialog.py

@@ -156,7 +156,7 @@ def __init__(self, lighthouse_tab, *args):
             self._lighthouse_tab._helper.cf, self._sweep_angles_received_and_averaged_signal.emit)
 
         self._base_station_geometry_wizard = LighthouseBasestationGeometryWizard(
-            self._lighthouse_tab._helper.cf, self._base_station_geometery_received_signal.emit)
+            self._lighthouse_tab, self._base_station_geometery_received_signal.emit)
 
         self._lh_geos = None
         self._newly_estimated_geometry = {}

src/cfclient/ui/tabs/lighthouse_tab.py

@@ -31,6 +31,7 @@
 """
 
 import logging
+from enum import Enum
 
 from PyQt6 import uic
 from PyQt6.QtCore import Qt, pyqtSignal, QTimer
@@ -41,11 +42,14 @@
 import cfclient
 from cfclient.ui.tab_toolbox import TabToolbox
 
+from cfclient.ui.widgets.geo_estimator_widget import GeoEstimatorWidget
 from cflib.crazyflie.log import LogConfig
 from cflib.crazyflie.mem import LighthouseMemHelper
 from cflib.localization import LighthouseConfigWriter
 from cflib.localization import LighthouseConfigFileManager
 
+from cflib.crazyflie.mem.lighthouse_memory import LighthouseBsGeometry
+
 from cfclient.ui.dialogs.lighthouse_bs_geometry_dialog import LighthouseBsGeometryDialog
 from cfclient.ui.dialogs.basestation_mode_dialog import LighthouseBsModeDialog
 from cfclient.ui.dialogs.lighthouse_system_type_dialog import LighthouseSystemTypeDialog
@@ -260,6 +264,11 @@ def _mix(self, col1, col2, mix):
         return col1 * mix + col2 * (1.0 - mix)
 
 
+class UiMode(Enum):
+    flying = 1
+    geo_estimation = 2
+
+
 class LighthouseTab(TabToolbox, lighthouse_tab_class):
     """Tab for plotting Lighthouse data"""
 
@@ -295,6 +304,9 @@ def __init__(self, helper):
         super(LighthouseTab, self).__init__(helper, 'Lighthouse Positioning')
         self.setupUi(self)
 
+        self._geo_estimator_widget = GeoEstimatorWidget(self)
+        self._geometry_area.addWidget(self._geo_estimator_widget)
+
         # Always wrap callbacks from Crazyflie API though QT Signal/Slots
         # to avoid manipulating the UI when rendering it
         self._connected_signal.connect(self._connected)
@@ -355,10 +367,15 @@ def __init__(self, helper):
         self._load_sys_config_button.clicked.connect(self._load_sys_config_button_clicked)
         self._save_sys_config_button.clicked.connect(self._save_sys_config_button_clicked)
 
+        self._ui_mode = UiMode.flying
+        self._geo_mode_button.toggled.connect(lambda enabled: self._change_ui_mode(enabled))
+
         self._is_connected = False
         self._update_ui()
 
-    def write_and_store_geometry(self, geometries):
+    def write_and_store_geometry(self, geometries: dict[int, LighthouseBsGeometry]):
+        # TODO krri Handle repeated quick writes. This is called from the geo wizard and write_and_store_config() will
+        # throw if there is an ongoing write
         if self._lh_config_writer:
             self._lh_config_writer.write_and_store_config(self._new_system_config_written_to_cf_signal.emit,
                                                           geos=geometries)
@@ -386,6 +403,7 @@ def _connected(self, link_uri):
         logger.debug("Crazyflie connected to {}".format(link_uri))
 
         self._basestation_geometry_dialog.reset()
+        self._flying_mode_button.setChecked(True)
         self._is_connected = True
 
         if self._helper.cf.param.get_value('deck.bcLighthouse4') == '1':
@@ -481,6 +499,7 @@ def _disconnected(self, link_uri):
         self._update_graphics()
         self._plot_3d.clear()
         self._basestation_geometry_dialog.close()
+        self._flying_mode_button.setChecked(True)
         self.is_lighthouse_deck_active = False
         self._is_connected = False
         self._update_ui()
@@ -515,6 +534,14 @@ def _logging_error(self, log_conf, msg):
                           "Error when using log config",
                           " [{0}]: {1}".format(log_conf.name, msg))
 
+    def _change_ui_mode(self, is_geo_mode: bool):
+        if is_geo_mode:
+            self._ui_mode = UiMode.geo_estimation
+        else:
+            self._ui_mode = UiMode.flying
+
+        self._update_ui()
+
     def _update_graphics(self):
         if self.is_visible() and self.is_lighthouse_deck_active:
             self._plot_3d.update_cf_pose(self._helper.pose_logger.position,
@@ -532,6 +559,10 @@ def _update_ui(self):
         self._load_sys_config_button.setEnabled(enabled)
         self._save_sys_config_button.setEnabled(enabled)
 
+        self._mode_group.setEnabled(enabled)
+
+        self._geo_estimator_widget.setVisible(self._ui_mode == UiMode.geo_estimation and enabled)
+
     def _update_position_label(self, position):
         if len(position) == 3:
             coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format(