cfclient 2017.4__py3-none-any.whl → 2025.12.1__py3-none-any.whl

cfclient/ui/tabs/locopositioning_tab.py

@@ -7,7 +7,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2011-2017 Bitcraze AB
+#  Copyright (C) 2011-2023 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -34,43 +34,36 @@ import logging
 from enum import Enum
 from collections import namedtuple
 
-from PyQt5 import uic
-from PyQt5.QtCore import pyqtSignal, QTimer
-from PyQt5.QtGui import QFont
-from PyQt5.QtGui import QMessageBox
+import time
+from PyQt6 import uic
+from PyQt6.QtCore import Qt, pyqtSignal, QTimer
+from PyQt6.QtWidgets import QMessageBox
+from PyQt6.QtWidgets import QLabel
 
 import cfclient
-from cfclient.ui.tab import Tab
+from cfclient.ui.tab_toolbox import TabToolbox
 
 from cflib.crazyflie.log import LogConfig
 from cflib.crazyflie.mem import MemoryElement
 from lpslib.lopoanchor import LoPoAnchor
 
+from cfclient.ui.dialogs.anchor_position_dialog import AnchorPositionDialog
+
+from vispy import scene
+import numpy as np
+
 import copy
-import sys
 
 __author__ = 'Bitcraze AB'
 __all__ = ['LocoPositioningTab']
 
 logger = logging.getLogger(__name__)
 
-locopositioning_tab_class = uic.loadUiType(
-    cfclient.module_path + "/ui/tabs/locopositioning_tab.ui")[0]
-
-# Try the imports for PyQtGraph to see if it is installed
-try:
-    import pyqtgraph as pg
-    from pyqtgraph import ViewBox  # noqa
-    import pyqtgraph.console  # noqa
-    import numpy as np  # noqa
-
-    _pyqtgraph_found = True
-except Exception:
-    import traceback
+locopositioning_tab_class = uic.loadUiType(cfclient.module_path + "/ui/tabs/locopositioning_tab.ui")[0]
 
-    logger.warning("PyQtGraph (or dependency) failed to import:\n%s",
-                   traceback.format_exc())
-    _pyqtgraph_found = False
+STYLE_RED_BACKGROUND = "background-color: lightpink;"
+STYLE_GREEN_BACKGROUND = "background-color: lightgreen;"
+STYLE_NO_BACKGROUND = "background-color: none;"
 
 
 class Anchor:
@@ -78,6 +71,8 @@ class Anchor:
         self.x = x
         self.y = y
         self.z = z
+        self._is_position_valid = False
+        self._is_active = False
         self.distance = distance
 
     def set_position(self, position):
@@ -85,11 +80,21 @@ class Anchor:
         self.x = position[0]
         self.y = position[1]
         self.z = position[2]
+        self._is_position_valid = True
 
     def get_position(self):
         """Returns the position as a vector"""
         return (self.x, self.y, self.z)
 
+    def is_position_valid(self):
+        return self._is_position_valid
+
+    def set_is_active(self, is_active):
+        self._is_active = is_active
+
+    def is_active(self):
+        return self._is_active
+
 
 class AxisScaleStep:
     def __init__(self, from_view, from_axis, to_view, to_axis,
@@ -101,136 +106,158 @@ class AxisScaleStep:
         self.center_only = center_only
 
 
-class PlotWrapper:
-    XAxis = 0
-    YAxis = 1
-    _refs = []
-    _change_lock = False
-
-    axis_dict = {'x': 0, 'y': 1, 'z': 2}
-
-    ANCHOR_BRUSH = (60, 60, 60)
-    HIGHLIGHT_ANCHOR_BRUSH = (0, 255, 0)
-    POSITION_BRUSH = (0, 0, 255)
+class Plot3dLps(scene.SceneCanvas):
+    ANCHOR_BRUSH = np.array((0.2, 0.5, 0.2))
+    ANCHOR_BRUSH_INVALID = np.array((0.8, 0.5, 0.5))
+    HIGHLIGHT_ANCHOR_BRUSH = np.array((0, 1, 0))
+    POSITION_BRUSH = np.array((0, 0, 1.0))
 
     VICINITY_DISTANCE = 2.5
     HIGHLIGHT_DISTANCE = 0.5
 
-    LABEL_SIZE = 15
-    LABEL_HIGHLIGHT_SIZE = 30
+    LABEL_SIZE = 100
+    LABEL_HIGHLIGHT_SIZE = 200
 
     ANCHOR_SIZE = 10
     HIGHLIGHT_SIZE = 20
 
-    def __init__(self, title, horizontal, vertical):
-        self._horizontal = horizontal
-        self._vertical = vertical
-        self._depth = self._find_missing_axis(horizontal, vertical)
-        self._title = title
-        self.widget = pg.PlotWidget(title=title, enableMenu=False)
-        self.widget.getPlotItem().hideButtons()
-        self._axis_scale_steps = []
-
-        self.widget.setLabel('left', self._vertical, units='m')
-        self.widget.setLabel('bottom', self._horizontal, units='m')
-
-        self.widget.setAspectLocked(True, 1)
-        self.widget.getViewBox().sigRangeChanged.connect(self._view_changed)
-        self.view = self.widget.getViewBox()
-
-    def update(self, anchors, pos, display_mode):
-        self.widget.clear()
-
-        # Sort anchors in depth order to add the one closes last
-        for (i, anchor) in sorted(
-                anchors.items(),
-                key=lambda item: getattr(item[1], self._depth), reverse=True):
-            anchor_v = getattr(anchor, self._horizontal)
-            anchor_h = getattr(anchor, self._vertical)
-            self._plot_anchor(anchor_v, anchor_h, i, anchor.distance,
-                              display_mode)
-
-        if display_mode is DisplayMode.estimated_position:
-            cf_h = pos[PlotWrapper.axis_dict[self._horizontal]]
-            cf_v = pos[PlotWrapper.axis_dict[self._vertical]]
-            self.widget.plot([cf_h], [cf_v], pen=None,
-                             symbolBrush=PlotWrapper.POSITION_BRUSH)
-
-    def _find_missing_axis(self, axis1, axis2):
-        all = set(self.axis_dict.keys())
-        all.remove(axis1)
-        all.remove(axis2)
-
-        return list(all)[0]
-
-    def _plot_anchor(self, x, y, anchor_id, distance, display_mode):
-        brush = PlotWrapper.ANCHOR_BRUSH
-        size = PlotWrapper.ANCHOR_SIZE
+    TEXT_OFFSET = np.array((0.0, 0, 0.25))
+
+    def __init__(self):
+        scene.SceneCanvas.__init__(self, keys=None)
+        self.unfreeze()
+
+        self._view = self.central_widget.add_view()
+        self._view.bgcolor = '#ffffff'
+        self._view.camera = scene.TurntableCamera(
+            distance=10.0,
+            up='+z',
+            center=(0.0, 0.0, 1.0))
+
+        self._cf = scene.visuals.Markers(
+            pos=np.array([[0, 0, 0]]),
+            parent=self._view.scene,
+            face_color=self.POSITION_BRUSH)
+        self._anchor_contexts = {}
+
+        self.freeze()
+
+        plane_size = 10
+        scene.visuals.Plane(
+            width=plane_size,
+            height=plane_size,
+            width_segments=plane_size,
+            height_segments=plane_size,
+            color=(0.5, 0.5, 0.5, 0.5),
+            edge_color="gray",
+            parent=self._view.scene)
+
+        self.addArrows(1, 0.02, 0.1, 0.1, self._view.scene)
+
+    def addArrows(self, length, width, head_length, head_width, parent):
+        # The Arrow visual in vispy does not seem to work very good,
+        # draw arrows using lines instead.
+        w = width / 2
+        hw = head_width / 2
+        base_len = length - head_length
+
+        # X-axis
+        scene.visuals.LinePlot([
+            [0, w, 0],
+            [base_len, w, 0],
+            [base_len, hw, 0],
+            [length, 0, 0],
+            [base_len, -hw, 0],
+            [base_len, -w, 0],
+            [0, -w, 0]],
+            width=1.0, color='red', parent=parent, marker_size=0.0)
+
+        # Y-axis
+        scene.visuals.LinePlot([
+            [w, 0, 0],
+            [w, base_len, 0],
+            [hw, base_len, 0],
+            [0, length, 0],
+            [-hw, base_len, 0],
+            [-w, base_len, 0],
+            [-w, 0, 0]],
+            width=1.0, color='green', parent=parent, marker_size=0.0)
+
+        # Z-axis
+        scene.visuals.LinePlot([
+            [0, w, 0],
+            [0, w, base_len],
+            [0, hw, base_len],
+            [0, 0, length],
+            [0, -hw, base_len],
+            [0, -w, base_len],
+            [0, -w, 0]],
+            width=1.0, color='blue', parent=parent, marker_size=0.0)
+
+    def update_data(self, anchors, pos, display_mode):
+        self._cf.set_data(pos=np.array([pos]), face_color=self.POSITION_BRUSH)
+
+        for id, anchor in anchors.items():
+            self._update_anchor(id, anchor, display_mode)
+
+        self._purge_anchors(anchors.keys())
+
+    def _update_anchor(self, id, anchor, display_mode):
+        if anchor.is_active():
+            color = self.ANCHOR_BRUSH
+        else:
+            color = self.ANCHOR_BRUSH_INVALID
+
+        size = self.ANCHOR_SIZE
         font_size = self.LABEL_SIZE
+        distance = anchor.distance
         if display_mode is DisplayMode.identify_anchor:
-            if distance < PlotWrapper.VICINITY_DISTANCE:
-                brush = self._mix_brushes(
-                    brush,
-                    PlotWrapper.HIGHLIGHT_ANCHOR_BRUSH,
-                    distance / PlotWrapper.VICINITY_DISTANCE)
-
-            if distance < PlotWrapper.HIGHLIGHT_DISTANCE:
-                brush = PlotWrapper.HIGHLIGHT_ANCHOR_BRUSH
-                size = PlotWrapper.HIGHLIGHT_SIZE
+            if distance < self.VICINITY_DISTANCE:
+                amount = (distance - self.HIGHLIGHT_DISTANCE) / \
+                    (self.VICINITY_DISTANCE - self.HIGHLIGHT_DISTANCE)
+                color = self._mix(color, self.HIGHLIGHT_ANCHOR_BRUSH, amount)
+
+            if distance < self.HIGHLIGHT_DISTANCE:
+                color = self.HIGHLIGHT_ANCHOR_BRUSH
+                size = self.HIGHLIGHT_SIZE
                 font_size = self.LABEL_HIGHLIGHT_SIZE
 
-        self.widget.plot([x], [y], pen=None, symbolBrush=brush,
-                         symbolSize=size)
-
-        text = pg.TextItem(text="{}".format(anchor_id))
-        font = QFont("Helvetica", font_size)
-        text.setFont(font)
-        self.widget.addItem(text)
-        text.setPos(x, y)
-
-    def _mix_brushes(self, brush1, brush2, mix):
-        if mix < 0.0:
-            return brush1
-        if mix > 1.0:
-            return brush2
-
-        b1 = mix
-        b2 = 1.0 - mix
-        return (
-            brush1[0] * b1 + brush2[0] * b2,
-            brush1[1] * b1 + brush2[1] * b2,
-            brush1[2] * b1 + brush2[2] * b2,
-        )
-
-    def _view_changed(self, view, settings):
-        # Ignore all callbacks until this change is processed
-        if PlotWrapper._change_lock:
-            return
-        PlotWrapper._change_lock = True
-
-        for step in self._axis_scale_steps:
-            range = step.from_view.viewRange()[step.from_axis]
-            new_range = range
-
-            if step.center_only:
-                center = (range[0] + range[1]) / 2
-                current_range = step.to_view.viewRange()[step.to_axis]
-                current_center = (current_range[0] + current_range[1]) / 2
-                delta = center - current_center
-                new_range = [current_range[0] + delta,
-                             current_range[1] + delta]
-
-            if step.to_axis is PlotWrapper.XAxis:
-                step.to_view.setRange(xRange=new_range, padding=0.0,
-                                      update=True)
-            else:
-                step.to_view.setRange(yRange=new_range, padding=0.0,
-                                      update=True)
-
-        PlotWrapper._change_lock = False
-
-    def set_scale_steps(self, steps):
-        self._axis_scale_steps = steps
+        marker_pos = anchor.get_position()
+        text_pos = self.TEXT_OFFSET + marker_pos
+        if id in self._anchor_contexts:
+            self._anchor_contexts[id][0].set_data(
+                pos=np.array([marker_pos]),
+                face_color=color,
+                size=size)
+
+            text = self._anchor_contexts[id][1]
+            text.pos = text_pos
+            text.font_size = font_size
+        else:
+            marker = scene.visuals.Markers(
+                pos=np.array([marker_pos]),
+                face_color=color,
+                size=size,
+                parent=self._view.scene)
+            text = scene.visuals.Text(
+                text=str(id),
+                font_size=font_size,
+                pos=text_pos,
+                parent=self._view.scene)
+            self._anchor_contexts[id] = [marker, text]
+
+    def _purge_anchors(self, keep):
+        to_remove = []
+        for id, context in self._anchor_contexts.items():
+            if id not in keep:
+                to_remove.append(id)
+                for visual in context:
+                    visual.parent = None
+        for id in to_remove:
+            self._anchor_contexts.pop(id)
+
+    def _mix(self, col1, col2, mix):
+        return col1 * mix + col2 * (1.0 - mix)
 
 
 class DisplayMode(Enum):
@@ -241,38 +268,99 @@ class DisplayMode(Enum):
 Range = namedtuple('Range', ['min', 'max'])
 
 
-class AnchorPosWrapper():
-    """Wraps the UI elements of one anchor position"""
-    def __init__(self, x, y, z):
-        self._x = x
-        self._y = y
-        self._z = z
+class AnchorStateMachine:
+    GET_ACTIVE = 0
+    GET_IDS = 1
+    GET_DATA = 2
+    STEPS = [
+        GET_ACTIVE,
+        GET_ACTIVE,
+        GET_IDS,
+        GET_ACTIVE,
+        GET_ACTIVE,
+        GET_DATA,
+        GET_ACTIVE,
+        GET_ACTIVE,
+    ]
+
+    def __init__(self, mem_sub, cb_active_id_list, cb_id_list, cb_data):
+        self._current_step = 0
+        self._waiting_for_response = False
+        self._mem = self._get_mem(mem_sub)
+
+        self._cb_active_id_list = cb_active_id_list
+        self._cb_id_list = cb_id_list
+        self._cb_data = cb_data
+
+    def poll(self):
+        if not self._waiting_for_response:
+            self._next_step()
+            self._waiting_for_response = self._request_step()
+
+    def _next_step(self):
+        self._current_step += 1
+        if self._current_step >= len(AnchorStateMachine.STEPS):
+            self._current_step = 0
+
+    def _request_step(self):
+        result = True
+
+        action = AnchorStateMachine.STEPS[self._current_step]
+        if action == AnchorStateMachine.GET_ACTIVE:
+            self._mem.update_active_id_list(self._cb_active_id_list_updated)
+        elif action == AnchorStateMachine.GET_IDS:
+            self._mem.update_id_list(self._cb_id_list_updated)
+        else:
+            if self._mem.nr_of_anchors > 0:
+                # Only request anchor data if we actually have anchors, otherwise the callback will never be called
+                self._mem.update_data(self._cb_data_updated)
+            else:
+                result = False
 
-    def get_position(self):
-        """Get the position from the UI elements"""
-        return (self._x.value(), self._y.value(), self._z.value())
+        return result
 
-    def set_position(self, position):
-        """Set the position in the UI elements"""
-        self._x.setValue(position[0])
-        self._y.setValue(position[1])
-        self._z.setValue(position[2])
+    def _get_mem(self, mem_sub):
+        mem = mem_sub.get_mems(MemoryElement.TYPE_LOCO2)
+        if len(mem) > 0:
+            return mem[0]
+        return None
+
+    def _cb_active_id_list_updated(self, mem_data):
+        self._waiting_for_response = False
+        if self._cb_active_id_list:
+            self._cb_active_id_list(mem_data.active_anchor_ids)
 
-    def enable(self, enabled):
-        """Enable/disable all UI elements for the position"""
-        self._x.setEnabled(enabled)
-        self._y.setEnabled(enabled)
-        self._z.setEnabled(enabled)
+    def _cb_id_list_updated(self, mem_data):
+        self._waiting_for_response = False
+        if self._cb_id_list:
+            self._cb_id_list(mem_data.anchor_ids)
 
+    def _cb_data_updated(self, mem_data):
+        self._waiting_for_response = False
+        if self._cb_data:
+            self._cb_data(mem_data.anchor_data)
 
-class LocoPositioningTab(Tab, locopositioning_tab_class):
+
+class LocoPositioningTab(TabToolbox, locopositioning_tab_class):
     """Tab for plotting Loco Positioning data"""
 
     # Update period of log data in ms
     UPDATE_PERIOD_LOG = 100
 
     # Update period of anchor position data
-    UPDATE_PERIOD_ANCHOR_POS = 5000
+    UPDATE_PERIOD_ANCHOR_STATE = 1000
+
+    UPDATE_PERIOD_LOCO_MODE = 1000
+
+    LOCO_MODE_UNKNOWN = -1
+    LOCO_MODE_AUTO = 0
+    LOCO_MODE_TWR = 1
+    LOCO_MODE_TDOA2 = 2
+    LOCO_MODE_TDOA3 = 3
+
+    PARAM_MDOE_GR = 'loco'
+    PARAM_MODE_NM = 'mode'
+    PARAM_MODE = PARAM_MDOE_GR + '.' + PARAM_MODE_NM
 
     # Frame rate (updates per second)
     FPS = 2
@@ -281,21 +369,17 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
     _disconnected_signal = pyqtSignal(str)
     _log_error_signal = pyqtSignal(object, str)
     _anchor_range_signal = pyqtSignal(int, object, object)
-    _position_signal = pyqtSignal(int, object, object)
-    _anchor_position_signal = pyqtSignal(object)
-
-    def __init__(self, tabWidget, helper, *args):
-        super(LocoPositioningTab, self).__init__(*args)
-        self.setupUi(self)
+    _loco_sys_signal = pyqtSignal(int, object, object)
+    _cb_param_to_detect_loco_deck_signal = pyqtSignal(object, object)
 
-        self.tabName = "Loco Positioning"
-        self.menuName = "Loco Positioning Tab"
-        self.tabWidget = tabWidget
+    _anchor_active_id_list_updated_signal = pyqtSignal(object)
+    _anchor_data_updated_signal = pyqtSignal(object)
 
-        self._helper = helper
+    def __init__(self, helper):
+        super(LocoPositioningTab, self).__init__(helper, 'Loco Positioning')
+        self.setupUi(self)
 
         self._anchors = {}
-        self._position = []
         self._clear_state()
         self._refs = []
 
@@ -306,34 +390,70 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
         self._connected_signal.connect(self._connected)
         self._disconnected_signal.connect(self._disconnected)
         self._anchor_range_signal.connect(self._anchor_range_received)
-        self._position_signal.connect(self._position_received)
-        self._anchor_position_signal.connect(self._anchor_positions_updated)
+        self._loco_sys_signal.connect(self._loco_sys_received)
+        self._cb_param_to_detect_loco_deck_signal.connect(
+            self._cb_param_to_detect_loco_deck)
+
+        self._anchor_active_id_list_updated_signal.connect(
+            self._active_id_list_updated)
+        self._anchor_data_updated_signal.connect(
+            self._anchor_data_updated)
 
-        self._id_anchor_button.clicked.connect(
+        self._id_anchor_button.toggled.connect(
             lambda enabled:
-            self._set_display_mode(DisplayMode.identify_anchor)
+            self._do_when_checked(
+                enabled,
+                self._set_display_mode,
+                DisplayMode.identify_anchor)
         )
 
-        self._estimated_postion_button.clicked.connect(
+        self._estimated_postion_button.toggled.connect(
             lambda enabled:
-            self._set_display_mode(DisplayMode.estimated_position)
+            self._do_when_checked(
+                enabled,
+                self._set_display_mode,
+                DisplayMode.estimated_position)
         )
 
-        self._anchor_pos_ui = {}
-        for anchor_nr in range(0, 8):
-            self._register_anchor_pos_ui(anchor_nr)
+        self._mode_auto.toggled.connect(
+            lambda enabled: self._request_mode(enabled, self.LOCO_MODE_AUTO)
+        )
 
-        self._write_pos_to_anhors_button.clicked.connect(
-            lambda enabled:
-            self._write_positions_to_anchors()
+        self._mode_twr.toggled.connect(
+            lambda enabled: self._request_mode(enabled, self.LOCO_MODE_TWR)
         )
 
-        self._read_pos_from_anhors_button.clicked.connect(
+        self._mode_tdoa2.toggled.connect(
+            lambda enabled: self._request_mode(enabled, self.LOCO_MODE_TDOA2)
+        )
+
+        self._mode_tdoa3.toggled.connect(
+            lambda enabled: self._request_mode(enabled, self.LOCO_MODE_TDOA3)
+        )
+
+        self._enable_mode_buttons(False)
+
+        self._switch_mode_to_twr_button.setEnabled(False)
+        self._switch_mode_to_tdoa2_button.setEnabled(False)
+        self._switch_mode_to_tdoa3_button.setEnabled(False)
+
+        self._switch_mode_to_twr_button.clicked.connect(
             lambda enabled:
-            self._read_positions_from_anchors()
+            self._send_anchor_mode(self.LOCO_MODE_TWR)
         )
+        self._switch_mode_to_tdoa2_button.clicked.connect(
+            lambda enabled:
+            self._send_anchor_mode(self.LOCO_MODE_TDOA2)
+        )
+        self._switch_mode_to_tdoa3_button.clicked.connect(
+            lambda enabled:
+            self._send_anchor_mode(self.LOCO_MODE_TDOA3)
+        )
+
+        self._clear_anchors_button.clicked.connect(self._clear_anchors)
 
-        self._show_all_button.clicked.connect(self._scale_and_center_graphs)
+        self._configure_anchor_positions_button.clicked.connect(
+            self._show_anchor_postion_dialog)
 
         # Connect the Crazyflie API callbacks to the signals
         self._helper.cf.connected.add_callback(
@@ -344,193 +464,96 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
 
         self._set_up_plots()
 
+        self.is_loco_deck_active = False
+
         self._graph_timer = QTimer()
-        self._graph_timer.setInterval(1000 / self.FPS)
+        self._graph_timer.setInterval(int(1000 / self.FPS))
         self._graph_timer.timeout.connect(self._update_graphics)
         self._graph_timer.start()
 
-        self._anchor_pos_timer = QTimer()
-        self._anchor_pos_timer.setInterval(self.UPDATE_PERIOD_ANCHOR_POS)
-        self._anchor_pos_timer.timeout.connect(self._poll_anchor_positions)
-
-    def _register_anchor_pos_ui(self, nr):
-        x_spin = getattr(self, 'spin_a{}x'.format(nr))
-        y_spin = getattr(self, 'spin_a{}y'.format(nr))
-        z_spin = getattr(self, 'spin_a{}z'.format(nr))
-        self._anchor_pos_ui[nr] = AnchorPosWrapper(x_spin, y_spin, z_spin)
-
-    def _write_positions_to_anchors(self):
-        lopo = LoPoAnchor(self._helper.cf)
+        self._anchor_state_timer = QTimer()
+        self._anchor_state_timer.setInterval(self.UPDATE_PERIOD_ANCHOR_STATE)
+        self._anchor_state_timer.timeout.connect(self._poll_anchor_state)
+        self._anchor_state_machine = None
 
-        for id, anchor_pos in self._anchor_pos_ui.items():
-            if id in self._anchors:
-                position = anchor_pos.get_position()
-                lopo.set_position(id, position)
+        self._update_position_label(self._helper.pose_logger.position)
 
-    def _read_positions_from_anchors(self):
-        for id, anchor_pos in self._anchor_pos_ui.items():
-            position = (0.0, 0.0, 0.0)
-            if id in self._anchors:
-                position = self._anchors[id].get_position()
+        self._lps_state = self.LOCO_MODE_UNKNOWN
+        self._update_lps_state(self.LOCO_MODE_UNKNOWN)
 
-            anchor_pos.set_position(position)
+        self._anchor_position_dialog = AnchorPositionDialog(self, helper)
+        self._configure_anchor_positions_button.setEnabled(False)
 
-    def _enable_anchor_pos_ui(self):
-        for id, anchor_pos in self._anchor_pos_ui.items():
-            exists = id in self._anchors
-            anchor_pos.enable(exists)
+    def _do_when_checked(self, enabled, fkn, arg):
+        if enabled:
+            fkn(arg)
 
     def _set_up_plots(self):
-        self._plot_xy = PlotWrapper("Top view (X/Y)", "x", "y")
-        self._plot_top_left_layout.addWidget(self._plot_xy.widget)
-        self._plot_xz = PlotWrapper("Front view (X/Z)", "x", "z")
-        self._plot_bottom_left_layout.addWidget(self._plot_xz.widget)
-        self._plot_yz = PlotWrapper("Right view (Y/Z)", "y", "z")
-        self._plot_bottom_right_layout.addWidget(self._plot_yz.widget)
-        self._plot_xy.set_scale_steps([
-            AxisScaleStep(self._plot_xy, PlotWrapper.XAxis,
-                          self._plot_xz, PlotWrapper.XAxis),
-            AxisScaleStep(self._plot_xz, PlotWrapper.YAxis,
-                          self._plot_yz, PlotWrapper.YAxis),
-            AxisScaleStep(self._plot_xy, PlotWrapper.YAxis,
-                          self._plot_yz, PlotWrapper.XAxis, center_only=True)
-        ])
-        self._plot_xz.set_scale_steps([
-            AxisScaleStep(self._plot_xz, PlotWrapper.XAxis,
-                          self._plot_xy, PlotWrapper.XAxis),
-            AxisScaleStep(self._plot_xz, PlotWrapper.YAxis,
-                          self._plot_yz, PlotWrapper.YAxis),
-            AxisScaleStep(self._plot_xy, PlotWrapper.YAxis,
-                          self._plot_yz, PlotWrapper.XAxis, center_only=True)
-        ])
-        self._plot_yz.set_scale_steps([
-            AxisScaleStep(self._plot_yz, PlotWrapper.YAxis,
-                          self._plot_xz, PlotWrapper.YAxis),
-            AxisScaleStep(self._plot_xz, PlotWrapper.XAxis,
-                          self._plot_xy, PlotWrapper.XAxis),
-            AxisScaleStep(self._plot_yz, PlotWrapper.XAxis,
-                          self._plot_xy, PlotWrapper.YAxis, center_only=True)
-        ])
-
-        self._plot_xy.view.setRange(xRange=(0.0, 5.0))
+        self._plot_3d = Plot3dLps()
+        self._plot_layout.addWidget(self._plot_3d.native)
 
     def _set_display_mode(self, display_mode):
         self._display_mode = display_mode
 
-    def _clear_state(self):
-        self._anchors = {}
-        self._position = [0.0, 0.0, 0.0]
-
-    def _scale_and_center_graphs(self):
-        start_bounds = Range(sys.float_info.max, -sys.float_info.max)
-        bounds = {"x": start_bounds, "y": start_bounds,
-                  "z": start_bounds}
-        for a in self._anchors.values():
-            bounds = self._find_min_max_data_range(bounds, [a.x, a.y, a.z])
-        bounds = self._find_min_max_data_range(bounds, self._position)
-
-        bounds = self._pad_bounds(bounds)
-        self._center_all_data_in_graphs(bounds)
-        self._rescale_to_fit_data(bounds)
-
-    def _rescale_to_fit_data(self, bounds):
-        [[xy_xmin, xy_xmax],
-            [xy_ymin, xy_ymax]] = self._plot_xy.view.viewRange()
-        [[yz_xmin, yz_xmax],
-            [yz_ymin, yz_ymax]] = self._plot_yz.view.viewRange()
-        if not self._is_data_visibile(bounds, self._position):
-            if self._will_new_range_zoom_in(Range(xy_xmin, xy_xmax),
-                                            bounds["x"]):
-                self._plot_xy.view.setRange(xRange=bounds["x"],
-                                            padding=0.0, update=True)
-
-        if not self._is_data_visibile(bounds, self._position):
-            if self._will_new_range_zoom_in(Range(xy_ymin, xy_ymax),
-                                            bounds["y"]):
-                self._plot_xy.view.setRange(yRange=bounds["y"],
-                                            padding=0.0, update=True)
-
-        if not self._is_data_visibile(bounds, self._position):
-            if self._will_new_range_zoom_in(Range(yz_xmin, yz_xmax),
-                                            bounds["y"]):
-                self._plot_yz.view.setRange(yRange=bounds["y"],
-                                            padding=0.0, update=True)
-
-        if not self._is_data_visibile(bounds, self._position):
-            if self._will_new_range_zoom_in(Range(yz_ymin, yz_ymax),
-                                            bounds["z"]):
-                self._plot_yz.view.setRange(yRange=bounds["z"], padding=0.0,
-                                            update=True)
-
-    def _pad_bounds(self, ranges):
-        new_ranges = ranges
-
-        new_ranges["x"] = Range(new_ranges["x"].min - 1.0,
-                                new_ranges["x"].max + 1.0)
-
-        new_ranges["y"] = Range(new_ranges["y"].min - 1.0,
-                                new_ranges["y"].max + 1.0)
-
-        new_ranges["z"] = Range(new_ranges["z"].min - 1.0,
-                                new_ranges["z"].max + 1.0)
-
-        return new_ranges
-
-    def _center_all_data_in_graphs(self, ranges):
-        # Will center data in graphs without taking care of scaling
-        self._plot_xy.view.setRange(xRange=ranges["x"], yRange=ranges["y"],
-                                    padding=0.0, update=True)
-        self._plot_yz.view.setRange(yRange=ranges["z"], padding=0.0,
-                                    update=True)
-
-    def _will_new_range_zoom_in(self, old_range, new_range):
-        return old_range.min > new_range.min
-
-    def _is_data_visibile(self, ranges, point):
-        [[xy_xmin, xy_xmax],
-            [xy_ymin, xy_ymax]] = self._plot_xy.view.viewRange()
-        [[yz_xmin, yz_xmax],
-            [yz_ymin, yz_ymax]] = self._plot_yz.view.viewRange()
-        [[xz_xmin, xz_xmax],
-            [xz_ymin, xz_ymax]] = self._plot_xz.view.viewRange()
-
-        allVisible = True
-
-        if ranges["x"].min < xy_xmin or ranges["x"].max > xy_xmax:
-            allVisible = False
-
-        if ranges["z"].min < yz_ymin or ranges["z"].max > yz_ymax:
-            allVisible = False
-
-        if ranges["y"].min < yz_xmin or ranges["y"].max > yz_xmax:
-            allVisible = False
-
-        if ranges["y"].min < xy_ymin or ranges["y"].max > xy_ymax:
-            allVisible = False
-
-        return allVisible
-
-    def _find_min_max_data_range(self, ranges, point):
-        result = ranges
+    def _send_anchor_mode(self, mode):
+        lopo = LoPoAnchor(self._helper.cf)
 
-        result["x"] = Range(min(ranges["x"].min, point[0]),
-                            max(ranges["x"].max, point[0]))
+        mode_translation = {
+            self.LOCO_MODE_TWR: lopo.MODE_TWR,
+            self.LOCO_MODE_TDOA2: lopo.MODE_TDOA,
+            self.LOCO_MODE_TDOA3: lopo.MODE_TDOA3,
+        }
 
-        result["y"] = Range(min(ranges["y"].min, point[1]),
-                            max(ranges["y"].max, point[1]))
+        # Set the mode from the last to the first anchor
+        # In TDoA 2 mode this ensures that the master anchor is set last
+        # Note: We only switch mode of anchor 0 - 7 since this is what is
+        # supported in TWR and TDoA 2
+        for j in range(5):
+            for i in reversed(range(8)):
+                lopo.set_mode(i, mode_translation[mode])
 
-        result["z"] = Range(min(ranges["z"].min, point[2]),
-                            max(ranges["z"].max, point[2]))
+    def _clear_state(self):
+        self._clear_anchors()
+        self._update_ranging_status_indicators()
+        self._id_anchor_button.setEnabled(True)
 
-        return result
+    def _clear_anchors(self):
+        self._anchors = {}
 
     def _connected(self, link_uri):
         """Callback when the Crazyflie has been connected"""
         logger.debug("Crazyflie connected to {}".format(link_uri))
-
-        self._clear_state()
-
-        if self._helper.cf.mem.ow_search(vid=0xBC, pid=0x06):
+        self._request_param_to_detect_loco_deck()
+
+    def _request_param_to_detect_loco_deck(self):
+        """Send a parameter request to detect if the Loco deck is installed"""
+        group = 'deck'
+
+        def register(group, param):
+            if self._is_in_param_toc(group, param):
+                logger.debug("Requesting loco deck parameter")
+                self._helper.cf.param.add_update_callback(group=group,
+                                                          name=param,
+                                                          cb=self._cb_param_to_detect_loco_deck_signal.emit)
+
+        register(group, 'bcLoco')
+        register(group, 'bcDWM1000')  # For backwards compatibility
+
+    def _cb_param_to_detect_loco_deck(self, name, value):
+        """Callback from the parameter sub system when the Loco deck detection
+        parameter has been updated"""
+        if value == '1':
+            logger.debug("Loco deck installed, enabling LPS tab")
+            self._loco_deck_detected()
+        else:
+            logger.debug("No Loco deck installed")
+
+    def _loco_deck_detected(self):
+        """Called when the loco deck has been detected. Enables the tab,
+        starts logging and polling of the memory sub system as well as starts
+        timers for updating graphics"""
+        if not self.is_loco_deck_active:
+            self.is_loco_deck_active = True
             try:
                 self._register_logblock(
                     "LoPoTab0",
@@ -552,35 +575,58 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
                         ("ranging", "distance7", "float"),
                     ],
                     self._anchor_range_signal.emit,
-                    self._log_error_signal.emit),
+                    self._log_error_signal.emit)
 
                 self._register_logblock(
-                    "LoPoTab2",
+                    "LoPoSys",
                     [
-                        ("kalman", "stateX", "float"),
-                        ("kalman", "stateY", "float"),
-                        ("kalman", "stateZ", "float"),
+                        ("loco", "mode", "uint8_t")
                     ],
-                    self._position_signal.emit,
-                    self._log_error_signal.emit),
+                    self._loco_sys_signal.emit,
+                    self._log_error_signal.emit,
+                    update_period=self.UPDATE_PERIOD_LOCO_MODE)
             except KeyError as e:
                 logger.warning(str(e))
             except AttributeError as e:
                 logger.warning(str(e))
 
             self._start_polling_anchor_pos(self._helper.cf)
+            self._enable_mode_buttons(True)
+            self._configure_anchor_positions_button.setEnabled(True)
+
+            self._helper.cf.param.add_update_callback(
+                group=self.PARAM_MDOE_GR,
+                name=self.PARAM_MODE_NM,
+                cb=self._loco_mode_updated)
+
+            if self.PARAM_MDOE_GR in self._helper.cf.param.values:
+                if self.PARAM_MODE_NM in \
+                        self._helper.cf.param.values[self.PARAM_MDOE_GR]:
+                    self._loco_mode_updated(
+                        self.PARAM_MODE,
+                        self._helper.cf.param.values[self.PARAM_MDOE_GR][
+                            self.PARAM_MODE_NM])
 
     def _disconnected(self, link_uri):
         """Callback for when the Crazyflie has been disconnected"""
         logger.debug("Crazyflie disconnected from {}".format(link_uri))
         self._stop_polling_anchor_pos()
-
-    def _register_logblock(self, logblock_name, variables, data_cb, error_cb):
+        self._clear_state()
+        self._update_graphics()
+        self.is_loco_deck_active = False
+        self._update_lps_state(self.LOCO_MODE_UNKNOWN)
+        self._enable_mode_buttons(False)
+        self._loco_mode_updated('', self.LOCO_MODE_UNKNOWN)
+        self._configure_anchor_positions_button.setEnabled(False)
+        self._anchor_position_dialog.close()
+
+    def _register_logblock(self, logblock_name, variables, data_cb, error_cb,
+                           update_period=UPDATE_PERIOD_LOG):
         """Register log data to listen for. One logblock can contain a limited
         number of parameters (6 for floats)."""
-        lg = LogConfig(logblock_name, self.UPDATE_PERIOD_LOG)
+        lg = LogConfig(logblock_name, update_period)
         for variable in variables:
-            if self._is_in_toc(variable):
+            if self._is_in_log_toc(variable):
                 lg.add_variable('{}.{}'.format(variable[0], variable[1]),
                                 variable[2])
 
@@ -590,25 +636,79 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
         lg.start()
         return lg
 
-    def _is_in_toc(self, variable):
+    def _is_in_log_toc(self, variable):
         toc = self._helper.cf.log.toc
         group = variable[0]
         param = variable[1]
         return group in toc.toc and param in toc.toc[group]
 
+    def _is_in_param_toc(self, group, param):
+        toc = self._helper.cf.param.toc
+        return bool(group in toc.toc and param in toc.toc[group])
+
     def _anchor_range_received(self, timestamp, data, logconf):
         """Callback from the logging system when a range is updated."""
         for name, value in data.items():
             valid, anchor_number = self._parse_range_param_name(name)
-            if valid:
-                self._get_anchor(anchor_number).distance = float(value)
+            # Only set distance on anchors that we have seen through other
+            # messages to avoid creating anchor 0-7 even if they do not exist
+            # in a TDoA3 set up for instance
+            if self._anchor_exists(anchor_number):
+                if valid:
+                    anchor = self._get_create_anchor(anchor_number)
+                    anchor.distance = float(value)
+
+    def _loco_sys_received(self, timestamp, data, logconf):
+        """Callback from the logging system when the loco pos sys config
+        is updated."""
+        if self.PARAM_MODE in data:
+            lps_state = data[self.PARAM_MODE]
+            if lps_state == self.LOCO_MODE_TDOA2:
+                if self._id_anchor_button.isEnabled():
+                    if self._id_anchor_button.isChecked():
+                        self._estimated_postion_button.setChecked(True)
+                    self._id_anchor_button.setEnabled(False)
+            else:
+                if not self._id_anchor_button.isEnabled():
+                    self._id_anchor_button.setEnabled(True)
+            self._update_lps_state(lps_state)
 
-    def _position_received(self, timestamp, data, logconf):
-        """Callback from the logging system when the position is updated."""
-        for name, value in data.items():
-            valid, axis = self._parse_position_param_name(name)
-            if valid:
-                self._position[axis] = float(value)
+    def _update_ranging_status_indicators(self):
+        container = self._anchor_stats_container
+
+        ids = sorted(self._anchors.keys())
+
+        # Update existing labels or add new if needed
+        count = 0
+        for id in ids:
+            col = count % 8
+            row = int(count / 8)
+
+            if count < container.count():
+                label = container.itemAtPosition(row, col).widget()
+            else:
+                label = QLabel()
+                label.setMinimumSize(30, 0)
+                label.setProperty('frameShape', 'QFrame::Box')
+                label.setAlignment(Qt.AlignmentFlag.AlignCenter)
+                container.addWidget(label, row, col)
+
+            label.setText(str(id))
+
+            if self._anchors[id].is_active():
+                label.setStyleSheet(STYLE_GREEN_BACKGROUND)
+            else:
+                label.setStyleSheet(STYLE_RED_BACKGROUND)
+
+            count += 1
+
+        # Remove labels if there are too many
+        for i in range(count, container.count()):
+            col = i % 8
+            row = int(i / 8)
+
+            label = container.itemAtPosition(row, col).widget()
+            label.deleteLater()
 
     def _logging_error(self, log_conf, msg):
         """Callback from the log layer when an error occurs"""
@@ -619,24 +719,45 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
     def _start_polling_anchor_pos(self, crazyflie):
         """Set up a timer to poll anchor positions from the memory sub
         system"""
-        self._anchor_pos_timer.start()
+        if not self._anchor_state_machine:
+            self._anchor_state_machine = AnchorStateMachine(
+                crazyflie.mem,
+                self._anchor_active_id_list_updated_signal.emit,
+                None,
+                self._anchor_data_updated_signal.emit
+            )
+        self._anchor_state_timer.start()
 
     def _stop_polling_anchor_pos(self):
-        self._anchor_pos_timer.stop()
+        self._anchor_state_timer.stop()
+        self._anchor_state_machine = None
+
+    def _poll_anchor_state(self):
+        if self._anchor_state_machine:
+            self._anchor_state_machine.poll()
+
+    def _active_id_list_updated(self, anchor_list):
+        """Callback from the anchor state machine when we get a list of active
+        anchors"""
+        for id, anchor_data in self._anchors.items():
+            anchor_data.set_is_active(False)
+
+        for id in anchor_list:
+            anchor_data = self._get_create_anchor(id)
+            anchor_data.set_is_active(True)
 
-    def _poll_anchor_positions(self):
-        mems = self._helper.cf.mem.get_mems(MemoryElement.TYPE_LOCO)
-        if len(mems) > 0:
-            mems[0].update(self._anchor_position_signal.emit)
+        self._update_ranging_status_indicators()
 
-    def _anchor_positions_updated(self, mem):
-        """Callback from the memory sub system when the anchor positions
+    def _anchor_data_updated(self, position_dict):
+        """Callback from the anchor state machine when the anchor positions
          are updated"""
-        for anchor_number, anchor_data in enumerate(mem.anchor_data):
+        for id, anchor_data in position_dict.items():
+            anchor = self._get_create_anchor(id)
             if anchor_data.is_valid:
-                anchor = self._get_anchor(anchor_number)
                 anchor.set_position(anchor_data.position)
 
+        self._update_positions_in_config_dialog()
+
     def _parse_range_param_name(self, name):
         """Parse a parameter name for a ranging distance and return the number
            of the anchor. The name is on the format 'ranging.distance4' """
@@ -658,15 +779,111 @@ class LocoPositioningTab(Tab, locopositioning_tab_class):
             valid = True
         return (valid, axis)
 
-    def _get_anchor(self, anchor_number):
+    def _get_create_anchor(self, anchor_number):
         if anchor_number not in self._anchors:
             self._anchors[anchor_number] = Anchor()
         return self._anchors[anchor_number]
 
+    def _anchor_exists(self, anchor_number):
+        return anchor_number in self._anchors
+
     def _update_graphics(self):
-        if self.is_visible():
+        if self.is_visible() and self.is_loco_deck_active:
             anchors = copy.deepcopy(self._anchors)
-            self._plot_yz.update(anchors, self._position, self._display_mode)
-            self._plot_xy.update(anchors, self._position, self._display_mode)
-            self._plot_xz.update(anchors, self._position, self._display_mode)
-            self._enable_anchor_pos_ui()
+            self._plot_3d.update_data(
+                anchors,
+                self._helper.pose_logger.position,
+                self._display_mode)
+            self._update_position_label(self._helper.pose_logger.position)
+
+    def _update_position_label(self, position):
+        if len(position) == 3:
+            coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format(
+                position[0], position[1], position[2])
+        else:
+            coordinate = '(0.00, 0.00, 0.00)'
+
+        self._status_position.setText(coordinate)
+
+    def _update_lps_state(self, state):
+        if state != self._lps_state:
+            self._update_lps_state_indicator(self._state_twr,
+                                             state == self.LOCO_MODE_TWR)
+            self._update_lps_state_indicator(self._state_tdoa2,
+                                             state == self.LOCO_MODE_TDOA2)
+            self._update_lps_state_indicator(self._state_tdoa3,
+                                             state == self.LOCO_MODE_TDOA3)
+        self._lps_state = state
+
+    def _update_lps_state_indicator(self, element, active):
+        if active:
+            element.setStyleSheet(STYLE_GREEN_BACKGROUND)
+        else:
+            element.setStyleSheet(STYLE_NO_BACKGROUND)
+
+    def _enable_mode_buttons(self, enabled):
+        self._mode_auto.setEnabled(enabled)
+        self._mode_twr.setEnabled(enabled)
+        self._mode_tdoa2.setEnabled(enabled)
+        self._mode_tdoa3.setEnabled(enabled)
+
+    def _request_mode(self, enabled, mode):
+        if enabled:
+            self._helper.cf.param.set_value(self.PARAM_MODE, str(mode))
+
+            if mode == self.LOCO_MODE_TWR:
+                self._switch_mode_to_twr_button.setEnabled(False)
+                self._switch_mode_to_tdoa2_button.setEnabled(True)
+                self._switch_mode_to_tdoa3_button.setEnabled(True)
+            elif mode == self.LOCO_MODE_TDOA2:
+                self._switch_mode_to_twr_button.setEnabled(True)
+                self._switch_mode_to_tdoa2_button.setEnabled(False)
+                self._switch_mode_to_tdoa3_button.setEnabled(True)
+            elif mode == self.LOCO_MODE_TDOA3:
+                self._switch_mode_to_twr_button.setEnabled(True)
+                self._switch_mode_to_tdoa2_button.setEnabled(True)
+                self._switch_mode_to_tdoa3_button.setEnabled(False)
+            else:
+                self._switch_mode_to_twr_button.setEnabled(False)
+                self._switch_mode_to_tdoa2_button.setEnabled(False)
+                self._switch_mode_to_tdoa3_button.setEnabled(False)
+
+    def _loco_mode_updated(self, name, value):
+        mode = int(value)
+        if mode == self.LOCO_MODE_AUTO:
+            if not self._mode_auto.isChecked():
+                self._mode_auto.setChecked(True)
+        elif mode == self.LOCO_MODE_TWR:
+            if not self._mode_twr.isChecked():
+                self._mode_twr.setChecked(True)
+        elif mode == self.LOCO_MODE_TDOA2:
+            if not self._mode_tdoa2.isChecked():
+                self._mode_tdoa2.setChecked(True)
+        elif mode == self.LOCO_MODE_TDOA3:
+            if not self._mode_tdoa3.isChecked():
+                self._mode_tdoa3.setChecked(True)
+        else:
+            self._mode_auto.setChecked(False)
+            self._mode_twr.setChecked(False)
+            self._mode_tdoa2.setChecked(False)
+            self._mode_tdoa3.setChecked(False)
+
+    def _show_anchor_postion_dialog(self):
+        self._anchor_position_dialog.show()
+
+    def _update_positions_in_config_dialog(self):
+        positions = {}
+
+        for id, anchor in self._anchors.items():
+            if anchor.is_position_valid():
+                positions[id] = (anchor.x, anchor.y, anchor.z)
+
+        self._anchor_position_dialog.anchor_postions_updated(positions)
+
+    def write_positions_to_anchors(self, anchor_positions):
+        lopo = LoPoAnchor(self._helper.cf)
+
+        for _ in range(3):
+            for id, position in anchor_positions.items():
+                lopo.set_position(id, position)
+            time.sleep(0.2)