From d11baaa00df2957bcbd85c96eb009b8a498942f1 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Thu, 15 Oct 2026 14:56:07 +0200 Subject: [PATCH 1/3] Updated Loco tab to use cflib2 --- .../ui/dialogs/anchor_position_dialog.py | 3 +- src/cfclient/ui/tabs/__init__.py | 4 +- src/cfclient/ui/tabs/locopositioning_tab.py | 398 ++++++------------ 3 files changed, 133 insertions(+), 272 deletions(-) diff --git a/src/cfclient/ui/dialogs/anchor_position_dialog.py b/src/cfclient/ui/dialogs/anchor_position_dialog.py index 8caa5536..ecc6f962 100644 --- a/src/cfclient/ui/dialogs/anchor_position_dialog.py +++ b/src/cfclient/ui/dialogs/anchor_position_dialog.py @@ -28,7 +28,6 @@ import logging import cfclient -from cfclient.utils.logconfigreader import FILE_REGEX_YAML from PySide6 import QtWidgets from PySide6.QtUiTools import loadUiType from PySide6.QtCore import QAbstractTableModel, Qt @@ -42,6 +41,8 @@ logger = logging.getLogger(__name__) +FILE_REGEX_YAML = "Config *.yaml;;All *.*" + (anchor_postiong_widget_class, connect_widget_base_class) = ( loadUiType( cfclient.module_path + '/ui/dialogs/anchor_position_dialog.ui') diff --git a/src/cfclient/ui/tabs/__init__.py b/src/cfclient/ui/tabs/__init__.py index 38986184..16d03de6 100644 --- a/src/cfclient/ui/tabs/__init__.py +++ b/src/cfclient/ui/tabs/__init__.py @@ -44,7 +44,7 @@ from .ParamTab import ParamTab # from .PlotTab import PlotTab -# from .locopositioning_tab import LocoPositioningTab +from .locopositioning_tab import LocoPositioningTab # from .LogClientTab import LogClientTab # from .lighthouse_tab import LighthouseTab # from .TuningTab import TuningTab @@ -64,7 +64,7 @@ # LogTab, ParamTab, # PlotTab, - # LocoPositioningTab, + LocoPositioningTab, # LighthouseTab, # LogClientTab, # TuningTab, diff --git a/src/cfclient/ui/tabs/locopositioning_tab.py b/src/cfclient/ui/tabs/locopositioning_tab.py index 8fb82e24..52149fcb 100644 --- a/src/cfclient/ui/tabs/locopositioning_tab.py +++ b/src/cfclient/ui/tabs/locopositioning_tab.py @@ -30,22 +30,23 @@ Shows data for the Loco Positioning system """ +import asyncio import logging from enum import Enum from collections import namedtuple +from collections.abc import Callable + +import struct -import time from PySide6.QtUiTools import loadUiType -from PySide6.QtCore import Qt, Signal, QTimer -from PySide6.QtWidgets import QMessageBox +from PySide6.QtCore import Qt, QTimer from PySide6.QtWidgets import QLabel import cfclient 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.gui import create_task +from cflib2 import Crazyflie +from cflib2.error import DisconnectedError from cfclient.ui.dialogs.anchor_position_dialog import AnchorPositionDialog @@ -95,6 +96,21 @@ def set_is_active(self, is_active): def is_active(self): return self._is_active +class LoPoAnchor: + LPP_TYPE_POSITION = 1 + LPP_TYPE_REBOOT = 2 + LPP_TYPE_MODE = 3 + + def __init__(self, cf: Crazyflie) -> None: + self._lpp = cf.localization().loco_positioning() + + async def set_position(self, anchor_id: int, position: tuple[float, float, float]) -> None: + data = struct.pack(" None: + data = struct.pack("= 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 - - return result - - 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 _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(TabToolbox, locopositioning_tab_class): """Tab for plotting Loco Positioning data""" @@ -358,26 +301,23 @@ class LocoPositioningTab(TabToolbox, locopositioning_tab_class): LOCO_MODE_TDOA2 = 2 LOCO_MODE_TDOA3 = 3 - PARAM_MDOE_GR = 'loco' + PARAM_MODE_GR = 'loco' PARAM_MODE_NM = 'mode' - PARAM_MODE = PARAM_MDOE_GR + '.' + PARAM_MODE_NM + PARAM_MODE = PARAM_MODE_GR + '.' + PARAM_MODE_NM # Frame rate (updates per second) FPS = 2 - _connected_signal = Signal(str) - _disconnected_signal = Signal(str) - _log_error_signal = Signal(object, str) - _anchor_range_signal = Signal(int, object, object) - _loco_sys_signal = Signal(int, object, object) - _cb_param_to_detect_loco_deck_signal = Signal(object, object) - - _anchor_active_id_list_updated_signal = Signal(object) - _anchor_data_updated_signal = Signal(object) - def __init__(self, helper): super(LocoPositioningTab, self).__init__(helper, 'Loco Positioning') self.setupUi(self) + self._cf = None + self._on_connected_task = None + self._mode_task = None + self._anchor_mode_task = None + self._poll_task = None + self._log_tasks = [] + self._write_positions_task = None self._anchors = {} self._clear_state() @@ -385,19 +325,6 @@ def __init__(self, helper): self._display_mode = DisplayMode.estimated_position - # Always wrap callbacks from Crazyflie API though QT Signal/Slots - # to avoid manipulating the UI when rendering it - self._connected_signal.connect(self._connected) - self._disconnected_signal.connect(self._disconnected) - self._anchor_range_signal.connect(self._anchor_range_received) - 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.toggled.connect( lambda enabled: @@ -455,13 +382,6 @@ def __init__(self, helper): 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( - self._connected_signal.emit) - - self._helper.cf.disconnected.add_callback( - self._disconnected_signal.emit) - self._set_up_plots() self.is_loco_deck_active = False @@ -471,11 +391,6 @@ def __init__(self, helper): self._graph_timer.timeout.connect(self._update_graphics) self._graph_timer.start() - 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 - self._update_position_label(self._helper.pose_logger.position) self._lps_state = self.LOCO_MODE_UNKNOWN @@ -496,21 +411,18 @@ def _set_display_mode(self, display_mode): self._display_mode = display_mode def _send_anchor_mode(self, mode): - lopo = LoPoAnchor(self._helper.cf) - - mode_translation = { - self.LOCO_MODE_TWR: lopo.MODE_TWR, - self.LOCO_MODE_TDOA2: lopo.MODE_TDOA, - self.LOCO_MODE_TDOA3: lopo.MODE_TDOA3, - } + if self._cf is not None: + self._anchor_mode_task = create_task(self._send_anchor_mode_async(self._cf, mode)) + async def _send_anchor_mode_async(self, cf: Crazyflie, mode: int) -> None: + lopo = LoPoAnchor(cf) # 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]) + for _ in range(5): + for anchor_id in reversed(range(8)): + await lopo.set_mode(anchor_id, mode) def _clear_state(self): self._clear_anchors() @@ -520,97 +432,62 @@ def _clear_state(self): 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._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': + def connected(self, cf:Crazyflie) -> None: + """Called by TabToolbox when the Crazyflie has been connected""" + self._cf = cf + self._on_connected_task = create_task(self._on_connected(cf)) + + async def _on_connected(self, cf: Crazyflie) -> None: + param = cf.param() + names = param.names() + deck_present = False + + for name in ("deck.bcLoco", "deck.bcDWM1000"): + if name in names and int(await param.get(name)) == 1: + deck_present = True + + if deck_present: logger.debug("Loco deck installed, enabling LPS tab") - self._loco_deck_detected() + self._loco_deck_detected(cf) + if self.PARAM_MODE in names: + mode = await param.get(self.PARAM_MODE) + self._loco_mode_updated(self.PARAM_MODE, mode) + self._enable_mode_buttons(True) else: - logger.debug("No Loco deck installed") + logger.debug("No loco deck installed") - def _loco_deck_detected(self): + + def _loco_deck_detected(self, cf: Crazyflie) -> None: """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", - [ - ("ranging", "distance0", "float"), - ("ranging", "distance1", "float"), - ("ranging", "distance2", "float"), - ("ranging", "distance3", "float"), - ], - self._anchor_range_signal.emit, - self._log_error_signal.emit) - - self._register_logblock( - "LoPoTab1", - [ - ("ranging", "distance4", "float"), - ("ranging", "distance5", "float"), - ("ranging", "distance6", "float"), - ("ranging", "distance7", "float"), - ], - self._anchor_range_signal.emit, - self._log_error_signal.emit) - - self._register_logblock( - "LoPoSys", - [ - ("loco", "mode", "uint8_t") - ], - 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._log_tasks = [ + create_task(self._stream_log(cf, [f"ranging.distance{i}" for i in range(4)], + self.UPDATE_PERIOD_LOG, self._anchor_range_received)), + create_task(self._stream_log(cf, [f"ranging.distance{i}" for i in range(4, 8)], + self.UPDATE_PERIOD_LOG, self._anchor_range_received)), + create_task(self._stream_log(cf, [self.PARAM_MODE], + self.UPDATE_PERIOD_LOCO_MODE, self._loco_sys_received)), + ] + + self._poll_task = create_task(self._poll_anchors(cf)) 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 disconnected(self) -> None: + """Called by TabToolbox when the Crazyflie has been disconnected""" + for task in [self._on_connected_task, self._mode_task, self._anchor_mode_task, self._poll_task, *self._log_tasks, self._write_positions_task]: + if task is not None: + task.cancel() + self._on_connected_task = None + self._mode_task = None + self._anchor_mode_task = None + self._cf = None + self._poll_task = None + self._log_tasks = [] + self._write_positions_task = None + self._clear_state() self._update_graphics() self.is_loco_deck_active = False @@ -618,35 +495,30 @@ def _disconnected(self, link_uri): 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, update_period) + QTimer.singleShot(0, self._anchor_position_dialog.close) + + async def _stream_log(self, cf: Crazyflie, variables: list[str], period_ms: int, handler: Callable[[dict], None]) -> None: + log = cf.log() + names = log.names() + variables = [v for v in variables if v in names] + if not variables: + return + block = await log.create_block() for variable in variables: - if self._is_in_log_toc(variable): - lg.add_variable('{}.{}'.format(variable[0], variable[1]), - variable[2]) - - self._helper.cf.log.add_config(lg) - lg.data_received_cb.add_callback(data_cb) - lg.error_cb.add_callback(error_cb) - lg.start() - return lg - - 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): + await block.add_variable(variable) + stream = await block.start(period_ms) + + try: + while True: + data = await stream.next() + handler(data.data) + finally: + try: + await asyncio.shield(stream.stop()) + except (DisconnectedError, asyncio.CancelledError): + pass + + def _anchor_range_received(self, data: dict) -> None: """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) @@ -658,7 +530,7 @@ def _anchor_range_received(self, timestamp, data, logconf): anchor = self._get_create_anchor(anchor_number) anchor.distance = float(value) - def _loco_sys_received(self, timestamp, data, logconf): + def _loco_sys_received(self, data: dict) -> None: """Callback from the logging system when the loco pos sys config is updated.""" if self.PARAM_MODE in data: @@ -710,34 +582,15 @@ def _update_ranging_status_indicators(self): label = container.itemAtPosition(row, col).widget() label.deleteLater() - def _logging_error(self, log_conf, msg): - """Callback from the log layer when an error occurs""" - QMessageBox.about(self, "LocoPositioningTab error", - "Error when using log config", - " [{0}]: {1}".format(log_conf.name, msg)) - - def _start_polling_anchor_pos(self, crazyflie): - """Set up a timer to poll anchor positions from the memory sub - system""" - 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_state_timer.stop() - self._anchor_state_machine = None - - def _poll_anchor_state(self): - if self._anchor_state_machine: - self._anchor_state_machine.poll() + async def _poll_anchors(self, cf: Crazyflie) -> None: + while True: + lps = await cf.memory().read_loco_anchor_data() + self._active_id_list_updated(lps.active_anchor_ids) + self._anchor_data_updated(lps.anchors) + await asyncio.sleep(self.UPDATE_PERIOD_ANCHOR_STATE / 1000) def _active_id_list_updated(self, anchor_list): - """Callback from the anchor state machine when we get a list of active + """Called from the anchor polling when we get a list of active anchors""" for id, anchor_data in self._anchors.items(): anchor_data.set_is_active(False) @@ -749,7 +602,7 @@ def _active_id_list_updated(self, anchor_list): self._update_ranging_status_indicators() def _anchor_data_updated(self, position_dict): - """Callback from the anchor state machine when the anchor positions + """Called from the anchor polling when the anchor positions are updated""" for id, anchor_data in position_dict.items(): anchor = self._get_create_anchor(id) @@ -829,7 +682,8 @@ def _enable_mode_buttons(self, enabled): def _request_mode(self, enabled, mode): if enabled: - self._helper.cf.param.set_value(self.PARAM_MODE, str(mode)) + if self._cf is not None: + self._mode_task = create_task(self._set_mode_param(self._cf, mode)) if mode == self.LOCO_MODE_TWR: self._switch_mode_to_twr_button.setEnabled(False) @@ -848,6 +702,9 @@ def _request_mode(self, enabled, mode): self._switch_mode_to_tdoa2_button.setEnabled(False) self._switch_mode_to_tdoa3_button.setEnabled(False) + async def _set_mode_param(self, cf: Crazyflie, mode: int) -> None: + await cf.param().set(self.PARAM_MODE, mode) + def _loco_mode_updated(self, name, value): mode = int(value) if mode == self.LOCO_MODE_AUTO: @@ -881,9 +738,12 @@ def _update_positions_in_config_dialog(self): self._anchor_position_dialog.anchor_postions_updated(positions) def write_positions_to_anchors(self, anchor_positions): - lopo = LoPoAnchor(self._helper.cf) + if self._cf is not None: + self._write_positions_task = create_task(self._write_positions_to_anchors_async(self._cf, anchor_positions)) + async def _write_positions_to_anchors_async(self, cf: Crazyflie, anchor_positions: dict[int, tuple[float, float, float]]) -> None: + lopo = LoPoAnchor(cf) for _ in range(3): - for id, position in anchor_positions.items(): - lopo.set_position(id, position) - time.sleep(0.2) + for anchor_id, position in anchor_positions.items(): + await lopo.set_position(anchor_id, position) + await asyncio.sleep(0.2) From 83c6092daaba03269bd290a8624b15cd2a390ca4 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Thu, 15 Oct 2026 15:02:53 +0200 Subject: [PATCH 2/3] Re-formatted loco files --- .../ui/dialogs/anchor_position_dialog.py | 144 ++++--- src/cfclient/ui/tabs/__init__.py | 1 + src/cfclient/ui/tabs/locopositioning_tab.py | 377 ++++++++++-------- 3 files changed, 302 insertions(+), 220 deletions(-) diff --git a/src/cfclient/ui/dialogs/anchor_position_dialog.py b/src/cfclient/ui/dialogs/anchor_position_dialog.py index ecc6f962..beb7f215 100644 --- a/src/cfclient/ui/dialogs/anchor_position_dialog.py +++ b/src/cfclient/ui/dialogs/anchor_position_dialog.py @@ -25,32 +25,42 @@ """ Dialog box used to configure anchor positions. Used from the LPS tab. """ + +from __future__ import annotations + import logging +from typing import TYPE_CHECKING import cfclient from PySide6 import QtWidgets from PySide6.QtUiTools import loadUiType -from PySide6.QtCore import QAbstractTableModel, Qt +from PySide6.QtCore import QAbstractTableModel, QModelIndex, QObject, Qt from PySide6.QtGui import QBrush, QColor from PySide6.QtWidgets import QInputDialog, QFileDialog import yaml import os -__author__ = 'Bitcraze AB' -__all__ = ['AnchorPositionDialog'] +from cfclient.ui.pluginhelper import PluginHelper + +if TYPE_CHECKING: + from cfclient.ui.tabs.locopositioning_tab import LocoPositioningTab + +__author__ = "Bitcraze AB" +__all__ = ["AnchorPositionDialog"] logger = logging.getLogger(__name__) FILE_REGEX_YAML = "Config *.yaml;;All *.*" -(anchor_postiong_widget_class, connect_widget_base_class) = ( - loadUiType( - cfclient.module_path + '/ui/dialogs/anchor_position_dialog.ui') +Position = tuple[float, float, float] + +(anchor_postiong_widget_class, connect_widget_base_class) = loadUiType( + cfclient.module_path + "/ui/dialogs/anchor_position_dialog.ui" ) class AnchorPositionConfigTableModel(QAbstractTableModel): - def __init__(self, headers, parent=None, *args): + def __init__(self, headers: list[str], parent: QObject | None = None) -> None: QAbstractTableModel.__init__(self, parent) self._anchor_positions = [] self._headers = headers @@ -59,13 +69,15 @@ def __init__(self, headers, parent=None, *args): self._green_brush = QBrush(QColor(200, 255, 200)) self._red_brush = QBrush(QColor(255, 200, 200)) - def rowCount(self, parent=None, *args, **kwargs): + def rowCount(self, parent: QModelIndex | None = None) -> int: return len(self._anchor_positions) - def columnCount(self, parent=None, *args, **kwargs): + def columnCount(self, parent: QModelIndex | None = None) -> int: return len(self._headers) - def data(self, index, role=None): + def data( + self, index: QModelIndex, role: int | None = None + ) -> int | float | str | QBrush | None: value = self._anchor_positions[index.row()][index.column()] if index.isValid(): if index.column() == 0: @@ -76,27 +88,34 @@ def data(self, index, role=None): return value else: if role == Qt.ItemDataRole.DisplayRole: - return '%.2f' % (value) + return "%.2f" % (value) elif role == Qt.ItemDataRole.EditRole: return value elif role == Qt.ItemDataRole.BackgroundRole: return self._get_background(index.row(), index.column()) - return + return - def setData(self, index, value, role=Qt.ItemDataRole.EditRole): + def setData( + self, index: QModelIndex, value: float, role: int = Qt.ItemDataRole.EditRole + ) -> bool: if not index.isValid(): return False self._anchor_positions[index.row()][index.column()] = value return True - def headerData(self, col, orientation, role=None): - if orientation == Qt.Orientation.Horizontal and role == Qt.ItemDataRole.DisplayRole: + def headerData( + self, col: int, orientation: Qt.Orientation, role: int | None = None + ) -> str | None: + if ( + orientation == Qt.Orientation.Horizontal + and role == Qt.ItemDataRole.DisplayRole + ): return self._headers[col] - return + return - def flags(self, index): + def flags(self, index: QModelIndex) -> Qt.ItemFlag | None: if not index.isValid(): return None @@ -107,47 +126,50 @@ def flags(self, index): else: return Qt.ItemFlag.ItemIsEnabled | Qt.ItemFlag.ItemIsEditable - def add_anchor(self, anchor_id, x=0.0, y=0.0, z=0.0): + def add_anchor( + self, anchor_id: int, x: float = 0.0, y: float = 0.0, z: float = 0.0 + ) -> None: if not self._id_exist(anchor_id): self.layoutAboutToBeChanged.emit() self._anchor_positions.append([0, anchor_id, x, y, z]) self._anchor_positions.sort(key=lambda row: row[1]) self.layoutChanged.emit() - def replace_anchors_from_latest_known_positions(self): + def replace_anchors_from_latest_known_positions(self) -> None: self.replace_anchor_positions(self._latest_known_anchor_positions) - def replace_anchor_positions(self, anchor_positions): + def replace_anchor_positions(self, anchor_positions: dict[int, Position]) -> None: self.layoutAboutToBeChanged.emit() self._anchor_positions = [] for id, position in anchor_positions.items(): self.add_anchor(id, x=position[0], y=position[1], z=position[2]) self.layoutChanged.emit() - def get_anchor_postions(self): + def get_anchor_postions(self) -> dict[int, Position]: result = {} for row in self._anchor_positions: result[row[1]] = (row[2], row[3], row[4]) return result - def anchor_postions_updated(self, anchor_positions): + def anchor_postions_updated(self, anchor_positions: dict[int, Position]) -> None: self.layoutAboutToBeChanged.emit() self._latest_known_anchor_positions = anchor_positions self.layoutChanged.emit() - def remove_selected_anchors(self): + def remove_selected_anchors(self) -> None: self.layoutAboutToBeChanged.emit() - self._anchor_positions = list(filter( - lambda row: row[0] == 0, self._anchor_positions)) + self._anchor_positions = list( + filter(lambda row: row[0] == 0, self._anchor_positions) + ) self.layoutChanged.emit() - def _id_exist(self, anchor_id): + def _id_exist(self, anchor_id: int) -> bool: for anchor in self._anchor_positions: if anchor[1] == anchor_id: return True return False - def _get_background(self, row, col): + def _get_background(self, row: int, col: int) -> QBrush | None: id = self._anchor_positions[row][1] if id in self._latest_known_anchor_positions: current_value = self._anchor_positions[row][col] @@ -158,87 +180,95 @@ def _get_background(self, row, col): else: return self._red_brush - return + return class AnchorPositionDialog(QtWidgets.QWidget, anchor_postiong_widget_class): - - def __init__(self, lps_tab, helper): + def __init__(self, lps_tab: LocoPositioningTab, helper: PluginHelper) -> None: super(AnchorPositionDialog, self).__init__() self.setupUi(self) self._lps_tab = lps_tab self._helper = helper - self._headers = ['', 'id', 'x', 'y', 'z'] + self._headers = ["", "id", "x", "y", "z"] self._data_model = AnchorPositionConfigTableModel(self._headers, self) self._table_view.setModel(self._data_model) self._table_view.verticalHeader().setVisible(False) header = self._table_view.horizontalHeader() - header.setSectionResizeMode(0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) - header.setSectionResizeMode(1, QtWidgets.QHeaderView.ResizeMode.ResizeToContents) + header.setSectionResizeMode( + 0, QtWidgets.QHeaderView.ResizeMode.ResizeToContents + ) + header.setSectionResizeMode( + 1, QtWidgets.QHeaderView.ResizeMode.ResizeToContents + ) header.setSectionResizeMode(2, QtWidgets.QHeaderView.ResizeMode.Stretch) header.setSectionResizeMode(3, QtWidgets.QHeaderView.ResizeMode.Stretch) header.setSectionResizeMode(4, QtWidgets.QHeaderView.ResizeMode.Stretch) - self._add_anchor_button.clicked.connect( - self._add_anchor_button_clicked) + self._add_anchor_button.clicked.connect(self._add_anchor_button_clicked) self._remove_anchors_button.clicked.connect( - self._data_model.remove_selected_anchors) + self._data_model.remove_selected_anchors + ) self._get_from_anchors_button.clicked.connect( - self._get_from_anchors_button_clicked) + self._get_from_anchors_button_clicked + ) self._write_to_anchors_button.clicked.connect( - self._write_to_anchors_button_clicked) + self._write_to_anchors_button_clicked + ) self._close_button.clicked.connect(self.close) - self._load_button.clicked.connect( - self._load_button_clicked) - self._save_button.clicked.connect( - self._save_button_clicked) + self._load_button.clicked.connect(self._load_button_clicked) + self._save_button.clicked.connect(self._save_button_clicked) - def _add_anchor_button_clicked(self): + def _add_anchor_button_clicked(self) -> None: anchor_id, ok = QInputDialog.getInt( - self, "New anchor", "Enter id", min=0, max=255) + self, "New anchor", "Enter id", min=0, max=255 + ) if ok: self._data_model.add_anchor(anchor_id) - def _get_from_anchors_button_clicked(self): + def _get_from_anchors_button_clicked(self) -> None: self._data_model.replace_anchors_from_latest_known_positions() - def _write_to_anchors_button_clicked(self): + def _write_to_anchors_button_clicked(self) -> None: anchor_positions = self._data_model.get_anchor_postions() self._lps_tab.write_positions_to_anchors(anchor_positions) - def anchor_postions_updated(self, anchor_positions): + def anchor_postions_updated(self, anchor_positions: dict[int, Position]) -> None: self._data_model.anchor_postions_updated(anchor_positions) - def _load_button_clicked(self): - names = QFileDialog.getOpenFileName(self, 'Open file', self._helper.current_folder, FILE_REGEX_YAML) + def _load_button_clicked(self) -> None: + names = QFileDialog.getOpenFileName( + self, "Open file", self._helper.current_folder, FILE_REGEX_YAML + ) - if names[0] == '': + if names[0] == "": return self._helper.current_folder = os.path.dirname(names[0]) - f = open(names[0], 'r') + f = open(names[0], "r") with f: data = yaml.safe_load(f) anchor_positions = {} for id, pos in data.items(): - anchor_positions[id] = (pos['x'], pos['y'], pos['z']) + anchor_positions[id] = (pos["x"], pos["y"], pos["z"]) self._data_model.replace_anchor_positions(anchor_positions) - def _save_button_clicked(self): + def _save_button_clicked(self) -> None: anchor_positions = self._data_model.get_anchor_postions() data = {} for id, pos in anchor_positions.items(): - data[id] = {'x': pos[0], 'y': pos[1], 'z': pos[2]} + data[id] = {"x": pos[0], "y": pos[1], "z": pos[2]} - names = QFileDialog.getSaveFileName(self, 'Save file', self._helper.current_folder, FILE_REGEX_YAML) + names = QFileDialog.getSaveFileName( + self, "Save file", self._helper.current_folder, FILE_REGEX_YAML + ) - if names[0] == '': + if names[0] == "": return self._helper.current_folder = os.path.dirname(names[0]) @@ -248,6 +278,6 @@ def _save_button_clicked(self): else: filename = names[0] - f = open(filename, 'w') + f = open(filename, "w") with f: yaml.dump(data, f) diff --git a/src/cfclient/ui/tabs/__init__.py b/src/cfclient/ui/tabs/__init__.py index 16d03de6..36172e89 100644 --- a/src/cfclient/ui/tabs/__init__.py +++ b/src/cfclient/ui/tabs/__init__.py @@ -45,6 +45,7 @@ # from .PlotTab import PlotTab from .locopositioning_tab import LocoPositioningTab + # from .LogClientTab import LogClientTab # from .lighthouse_tab import LighthouseTab # from .TuningTab import TuningTab diff --git a/src/cfclient/ui/tabs/locopositioning_tab.py b/src/cfclient/ui/tabs/locopositioning_tab.py index 52149fcb..d5f5ff86 100644 --- a/src/cfclient/ui/tabs/locopositioning_tab.py +++ b/src/cfclient/ui/tabs/locopositioning_tab.py @@ -30,23 +30,26 @@ Shows data for the Loco Positioning system """ +from __future__ import annotations + import asyncio import logging from enum import Enum -from collections import namedtuple -from collections.abc import Callable +from collections.abc import Callable, Iterable, Sequence import struct from PySide6.QtUiTools import loadUiType from PySide6.QtCore import Qt, QTimer -from PySide6.QtWidgets import QLabel +from PySide6.QtWidgets import QLabel, QWidget import cfclient from cfclient.ui.tab_toolbox import TabToolbox from cfclient.gui import create_task +from cfclient.ui.pluginhelper import PluginHelper from cflib2 import Crazyflie from cflib2.error import DisconnectedError +from cflib2.memory import LocoAnchorData from cfclient.ui.dialogs.anchor_position_dialog import AnchorPositionDialog @@ -55,12 +58,14 @@ import copy -__author__ = 'Bitcraze AB' -__all__ = ['LocoPositioningTab'] +__author__ = "Bitcraze AB" +__all__ = ["LocoPositioningTab"] logger = logging.getLogger(__name__) -locopositioning_tab_class = loadUiType(cfclient.module_path + "/ui/tabs/locopositioning_tab.ui")[0] +locopositioning_tab_class = loadUiType( + cfclient.module_path + "/ui/tabs/locopositioning_tab.ui" +)[0] STYLE_RED_BACKGROUND = "background-color: lightpink;" STYLE_GREEN_BACKGROUND = "background-color: lightgreen;" @@ -68,7 +73,9 @@ class Anchor: - def __init__(self, x=0.0, y=0.0, z=0.0, distance=0.0): + def __init__( + self, x: float = 0.0, y: float = 0.0, z: float = 0.0, distance: float = 0.0 + ) -> None: self.x = x self.y = y self.z = z @@ -76,26 +83,27 @@ def __init__(self, x=0.0, y=0.0, z=0.0, distance=0.0): self._is_active = False self.distance = distance - def set_position(self, position): + def set_position(self, position: Sequence[float]) -> None: """Sets the position.""" self.x = position[0] self.y = position[1] self.z = position[2] self._is_position_valid = True - def get_position(self): + def get_position(self) -> tuple[float, float, float]: """Returns the position as a vector""" return (self.x, self.y, self.z) - def is_position_valid(self): + def is_position_valid(self) -> bool: return self._is_position_valid - def set_is_active(self, is_active): + def set_is_active(self, is_active: bool) -> None: self._is_active = is_active - def is_active(self): + def is_active(self) -> bool: return self._is_active + class LoPoAnchor: LPP_TYPE_POSITION = 1 LPP_TYPE_REBOOT = 2 @@ -104,7 +112,9 @@ class LoPoAnchor: def __init__(self, cf: Crazyflie) -> None: self._lpp = cf.localization().loco_positioning() - async def set_position(self, anchor_id: int, position: tuple[float, float, float]) -> None: + async def set_position( + self, anchor_id: int, position: tuple[float, float, float] + ) -> None: data = struct.pack(" None: data = struct.pack(" None: scene.SceneCanvas.__init__(self, keys=None) self.unfreeze() self._view = self.central_widget.add_view() - self._view.bgcolor = '#ffffff' + self._view.bgcolor = "#ffffff" self._view.camera = scene.TurntableCamera( - distance=10.0, - up='+z', - center=(0.0, 0.0, 1.0)) + 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) + face_color=self.POSITION_BRUSH, + ) self._anchor_contexts = {} self.freeze() @@ -166,11 +167,19 @@ def __init__(self): height_segments=plane_size, color=(0.5, 0.5, 0.5, 0.5), edge_color="gray", - parent=self._view.scene) + 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): + def addArrows( + self, + length: float, + width: float, + head_length: float, + head_width: float, + parent: scene.Node, + ) -> None: # The Arrow visual in vispy does not seem to work very good, # draw arrows using lines instead. w = width / 2 @@ -178,39 +187,59 @@ def addArrows(self, length, width, head_length, head_width, parent): 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) + 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) + 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): + 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: dict[int, Anchor], pos: list[float], display_mode: DisplayMode + ) -> None: self._cf.set_data(pos=np.array([pos]), face_color=self.POSITION_BRUSH) for id, anchor in anchors.items(): @@ -218,7 +247,9 @@ def update_data(self, anchors, pos, display_mode): self._purge_anchors(anchors.keys()) - def _update_anchor(self, id, anchor, display_mode): + def _update_anchor( + self, id: int, anchor: Anchor, display_mode: DisplayMode + ) -> None: if anchor.is_active(): color = self.ANCHOR_BRUSH else: @@ -229,8 +260,9 @@ def _update_anchor(self, id, anchor, display_mode): distance = anchor.distance if display_mode is DisplayMode.identify_anchor: if distance < self.VICINITY_DISTANCE: - amount = (distance - self.HIGHLIGHT_DISTANCE) / \ - (self.VICINITY_DISTANCE - self.HIGHLIGHT_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: @@ -242,9 +274,8 @@ def _update_anchor(self, id, anchor, display_mode): 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) + pos=np.array([marker_pos]), face_color=color, size=size + ) text = self._anchor_contexts[id][1] text.pos = text_pos @@ -254,15 +285,14 @@ def _update_anchor(self, id, anchor, display_mode): pos=np.array([marker_pos]), face_color=color, size=size, - parent=self._view.scene) + parent=self._view.scene, + ) text = scene.visuals.Text( - text=str(id), - font_size=font_size, - pos=text_pos, - parent=self._view.scene) + 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): + def _purge_anchors(self, keep: Iterable[int]) -> None: to_remove = [] for id, context in self._anchor_contexts.items(): if id not in keep: @@ -272,7 +302,7 @@ def _purge_anchors(self, keep): for id in to_remove: self._anchor_contexts.pop(id) - def _mix(self, col1, col2, mix): + def _mix(self, col1: np.ndarray, col2: np.ndarray, mix: float) -> np.ndarray: return col1 * mix + col2 * (1.0 - mix) @@ -281,9 +311,6 @@ class DisplayMode(Enum): estimated_position = 2 -Range = namedtuple('Range', ['min', 'max']) - - class LocoPositioningTab(TabToolbox, locopositioning_tab_class): """Tab for plotting Loco Positioning data""" @@ -301,17 +328,17 @@ class LocoPositioningTab(TabToolbox, locopositioning_tab_class): LOCO_MODE_TDOA2 = 2 LOCO_MODE_TDOA3 = 3 - PARAM_MODE_GR = 'loco' - PARAM_MODE_NM = 'mode' - PARAM_MODE = PARAM_MODE_GR + '.' + PARAM_MODE_NM + PARAM_MODE_GR = "loco" + PARAM_MODE_NM = "mode" + PARAM_MODE = PARAM_MODE_GR + "." + PARAM_MODE_NM # Frame rate (updates per second) FPS = 2 - def __init__(self, helper): - super(LocoPositioningTab, self).__init__(helper, 'Loco Positioning') + def __init__(self, helper: PluginHelper) -> None: + super(LocoPositioningTab, self).__init__(helper, "Loco Positioning") self.setupUi(self) - self._cf = None + self._cf: Crazyflie | None = None self._on_connected_task = None self._mode_task = None self._anchor_mode_task = None @@ -321,25 +348,19 @@ def __init__(self, helper): self._anchors = {} self._clear_state() - self._refs = [] self._display_mode = DisplayMode.estimated_position - self._id_anchor_button.toggled.connect( - lambda enabled: - self._do_when_checked( - enabled, - self._set_display_mode, - DisplayMode.identify_anchor) + lambda enabled: self._do_when_checked( + enabled, self._set_display_mode, DisplayMode.identify_anchor + ) ) self._estimated_postion_button.toggled.connect( - lambda enabled: - self._do_when_checked( - enabled, - self._set_display_mode, - DisplayMode.estimated_position) + lambda enabled: self._do_when_checked( + enabled, self._set_display_mode, DisplayMode.estimated_position + ) ) self._mode_auto.toggled.connect( @@ -365,22 +386,20 @@ def __init__(self, helper): self._switch_mode_to_tdoa3_button.setEnabled(False) self._switch_mode_to_twr_button.clicked.connect( - lambda enabled: - self._send_anchor_mode(self.LOCO_MODE_TWR) + lambda enabled: 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) + 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) + lambda enabled: self._send_anchor_mode(self.LOCO_MODE_TDOA3) ) self._clear_anchors_button.clicked.connect(self._clear_anchors) self._configure_anchor_positions_button.clicked.connect( - self._show_anchor_postion_dialog) + self._show_anchor_postion_dialog + ) self._set_up_plots() @@ -399,20 +418,24 @@ def __init__(self, helper): self._anchor_position_dialog = AnchorPositionDialog(self, helper) self._configure_anchor_positions_button.setEnabled(False) - def _do_when_checked(self, enabled, fkn, arg): + def _do_when_checked( + self, enabled: bool, fkn: Callable[[DisplayMode], None], arg: DisplayMode + ) -> None: if enabled: fkn(arg) - def _set_up_plots(self): + def _set_up_plots(self) -> None: self._plot_3d = Plot3dLps() self._plot_layout.addWidget(self._plot_3d.native) - def _set_display_mode(self, display_mode): + def _set_display_mode(self, display_mode: DisplayMode) -> None: self._display_mode = display_mode - def _send_anchor_mode(self, mode): + def _send_anchor_mode(self, mode: int) -> None: if self._cf is not None: - self._anchor_mode_task = create_task(self._send_anchor_mode_async(self._cf, mode)) + self._anchor_mode_task = create_task( + self._send_anchor_mode_async(self._cf, mode) + ) async def _send_anchor_mode_async(self, cf: Crazyflie, mode: int) -> None: lopo = LoPoAnchor(cf) @@ -424,15 +447,15 @@ async def _send_anchor_mode_async(self, cf: Crazyflie, mode: int) -> None: for anchor_id in reversed(range(8)): await lopo.set_mode(anchor_id, mode) - def _clear_state(self): + def _clear_state(self) -> None: self._clear_anchors() self._update_ranging_status_indicators() self._id_anchor_button.setEnabled(True) - def _clear_anchors(self): + def _clear_anchors(self) -> None: self._anchors = {} - def connected(self, cf:Crazyflie) -> None: + def connected(self, cf: Crazyflie) -> None: """Called by TabToolbox when the Crazyflie has been connected""" self._cf = cf self._on_connected_task = create_task(self._on_connected(cf)) @@ -456,7 +479,6 @@ async def _on_connected(self, cf: Crazyflie) -> None: else: logger.debug("No loco deck installed") - def _loco_deck_detected(self, cf: Crazyflie) -> None: """Called when the loco deck has been detected. Enables the tab, starts logging and polling of the memory sub system as well as starts @@ -464,12 +486,30 @@ def _loco_deck_detected(self, cf: Crazyflie) -> None: if not self.is_loco_deck_active: self.is_loco_deck_active = True self._log_tasks = [ - create_task(self._stream_log(cf, [f"ranging.distance{i}" for i in range(4)], - self.UPDATE_PERIOD_LOG, self._anchor_range_received)), - create_task(self._stream_log(cf, [f"ranging.distance{i}" for i in range(4, 8)], - self.UPDATE_PERIOD_LOG, self._anchor_range_received)), - create_task(self._stream_log(cf, [self.PARAM_MODE], - self.UPDATE_PERIOD_LOCO_MODE, self._loco_sys_received)), + create_task( + self._stream_log( + cf, + [f"ranging.distance{i}" for i in range(4)], + self.UPDATE_PERIOD_LOG, + self._anchor_range_received, + ) + ), + create_task( + self._stream_log( + cf, + [f"ranging.distance{i}" for i in range(4, 8)], + self.UPDATE_PERIOD_LOG, + self._anchor_range_received, + ) + ), + create_task( + self._stream_log( + cf, + [self.PARAM_MODE], + self.UPDATE_PERIOD_LOCO_MODE, + self._loco_sys_received, + ) + ), ] self._poll_task = create_task(self._poll_anchors(cf)) @@ -477,7 +517,14 @@ def _loco_deck_detected(self, cf: Crazyflie) -> None: def disconnected(self) -> None: """Called by TabToolbox when the Crazyflie has been disconnected""" - for task in [self._on_connected_task, self._mode_task, self._anchor_mode_task, self._poll_task, *self._log_tasks, self._write_positions_task]: + for task in [ + self._on_connected_task, + self._mode_task, + self._anchor_mode_task, + self._poll_task, + *self._log_tasks, + self._write_positions_task, + ]: if task is not None: task.cancel() self._on_connected_task = None @@ -493,11 +540,17 @@ def disconnected(self) -> None: 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._loco_mode_updated("", self.LOCO_MODE_UNKNOWN) self._configure_anchor_positions_button.setEnabled(False) QTimer.singleShot(0, self._anchor_position_dialog.close) - async def _stream_log(self, cf: Crazyflie, variables: list[str], period_ms: int, handler: Callable[[dict], None]) -> None: + async def _stream_log( + self, + cf: Crazyflie, + variables: list[str], + period_ms: int, + handler: Callable[[dict], None], + ) -> None: log = cf.log() names = log.names() variables = [v for v in variables if v in names] @@ -545,7 +598,7 @@ def _loco_sys_received(self, data: dict) -> None: self._id_anchor_button.setEnabled(True) self._update_lps_state(lps_state) - def _update_ranging_status_indicators(self): + def _update_ranging_status_indicators(self) -> None: container = self._anchor_stats_container ids = sorted(self._anchors.keys()) @@ -561,7 +614,7 @@ def _update_ranging_status_indicators(self): else: label = QLabel() label.setMinimumSize(30, 0) - label.setProperty('frameShape', 'QFrame::Box') + label.setProperty("frameShape", "QFrame::Box") label.setAlignment(Qt.AlignmentFlag.AlignCenter) container.addWidget(label, row, col) @@ -589,7 +642,7 @@ async def _poll_anchors(self, cf: Crazyflie) -> None: self._anchor_data_updated(lps.anchors) await asyncio.sleep(self.UPDATE_PERIOD_ANCHOR_STATE / 1000) - def _active_id_list_updated(self, anchor_list): + def _active_id_list_updated(self, anchor_list: list[int]) -> None: """Called from the anchor polling when we get a list of active anchors""" for id, anchor_data in self._anchors.items(): @@ -601,9 +654,9 @@ def _active_id_list_updated(self, anchor_list): self._update_ranging_status_indicators() - def _anchor_data_updated(self, position_dict): + def _anchor_data_updated(self, position_dict: dict[int, LocoAnchorData]) -> None: """Called from the anchor polling when the anchor positions - are updated""" + are updated""" for id, anchor_data in position_dict.items(): anchor = self._get_create_anchor(id) if anchor_data.is_valid: @@ -611,76 +664,68 @@ def _anchor_data_updated(self, position_dict): self._update_positions_in_config_dialog() - def _parse_range_param_name(self, name): + def _parse_range_param_name(self, name: str) -> tuple[bool, int]: """Parse a parameter name for a ranging distance and return the number - of the anchor. The name is on the format 'ranging.distance4' """ + of the anchor. The name is on the format 'ranging.distance4'""" valid = False anchor = 0 - if name.startswith('ranging.distance'): + if name.startswith("ranging.distance"): anchor = int(name[-1]) valid = True return (valid, anchor) - def _parse_position_param_name(self, name): - """Parse a parameter name for a position and return the - axis (0=x, 1=y, 2=z). - The param name is on the format 'kalman.stateY' """ - valid = False - axis = 0 - if name.startswith('kalman.state'): - axis = {'X': 0, 'Y': 1, 'Z': 2}[name[-1]] - valid = True - return (valid, axis) - - def _get_create_anchor(self, anchor_number): + def _get_create_anchor(self, anchor_number: int) -> Anchor: if anchor_number not in self._anchors: self._anchors[anchor_number] = Anchor() return self._anchors[anchor_number] - def _anchor_exists(self, anchor_number): + def _anchor_exists(self, anchor_number: int) -> bool: return anchor_number in self._anchors - def _update_graphics(self): + def _update_graphics(self) -> None: if self.is_visible() and self.is_loco_deck_active: anchors = copy.deepcopy(self._anchors) self._plot_3d.update_data( - anchors, - self._helper.pose_logger.position, - self._display_mode) + anchors, self._helper.pose_logger.position, self._display_mode + ) self._update_position_label(self._helper.pose_logger.position) - def _update_position_label(self, position): + def _update_position_label(self, position: list[float]) -> None: if len(position) == 3: coordinate = "({:0.2f}, {:0.2f}, {:0.2f})".format( - position[0], position[1], position[2]) + position[0], position[1], position[2] + ) else: - coordinate = '(0.00, 0.00, 0.00)' + coordinate = "(0.00, 0.00, 0.00)" self._status_position.setText(coordinate) - def _update_lps_state(self, state): + def _update_lps_state(self, state: int) -> None: 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._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): + def _update_lps_state_indicator(self, element: QWidget, active: bool) -> None: if active: element.setStyleSheet(STYLE_GREEN_BACKGROUND) else: element.setStyleSheet(STYLE_NO_BACKGROUND) - def _enable_mode_buttons(self, enabled): + def _enable_mode_buttons(self, enabled: bool) -> None: 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): + def _request_mode(self, enabled: bool, mode: int) -> None: if enabled: if self._cf is not None: self._mode_task = create_task(self._set_mode_param(self._cf, mode)) @@ -705,7 +750,7 @@ def _request_mode(self, enabled, mode): async def _set_mode_param(self, cf: Crazyflie, mode: int) -> None: await cf.param().set(self.PARAM_MODE, mode) - def _loco_mode_updated(self, name, value): + def _loco_mode_updated(self, name: str, value: int | float) -> None: mode = int(value) if mode == self.LOCO_MODE_AUTO: if not self._mode_auto.isChecked(): @@ -725,11 +770,11 @@ def _loco_mode_updated(self, name, value): self._mode_tdoa2.setChecked(False) self._mode_tdoa3.setChecked(False) - def _show_anchor_postion_dialog(self): + def _show_anchor_postion_dialog(self) -> None: self._anchor_position_dialog.show() - def _update_positions_in_config_dialog(self): - positions = {} + def _update_positions_in_config_dialog(self) -> None: + positions: dict[int, tuple[float, float, float]] = {} for id, anchor in self._anchors.items(): if anchor.is_position_valid(): @@ -737,11 +782,17 @@ def _update_positions_in_config_dialog(self): self._anchor_position_dialog.anchor_postions_updated(positions) - def write_positions_to_anchors(self, anchor_positions): + def write_positions_to_anchors( + self, anchor_positions: dict[int, tuple[float, float, float]] + ) -> None: if self._cf is not None: - self._write_positions_task = create_task(self._write_positions_to_anchors_async(self._cf, anchor_positions)) + self._write_positions_task = create_task( + self._write_positions_to_anchors_async(self._cf, anchor_positions) + ) - async def _write_positions_to_anchors_async(self, cf: Crazyflie, anchor_positions: dict[int, tuple[float, float, float]]) -> None: + async def _write_positions_to_anchors_async( + self, cf: Crazyflie, anchor_positions: dict[int, tuple[float, float, float]] + ) -> None: lopo = LoPoAnchor(cf) for _ in range(3): for anchor_id, position in anchor_positions.items(): From 437ea53d3bcf324ca29a987ce4772558e2ab59f7 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Thu, 15 Oct 2026 15:36:57 +0200 Subject: [PATCH 3/3] Skip anchors that have the position while writing --- .../ui/dialogs/anchor_position_dialog.py | 4 ++ src/cfclient/ui/tabs/locopositioning_tab.py | 51 ++++++++++++++++--- 2 files changed, 47 insertions(+), 8 deletions(-) diff --git a/src/cfclient/ui/dialogs/anchor_position_dialog.py b/src/cfclient/ui/dialogs/anchor_position_dialog.py index beb7f215..cb822e32 100644 --- a/src/cfclient/ui/dialogs/anchor_position_dialog.py +++ b/src/cfclient/ui/dialogs/anchor_position_dialog.py @@ -239,6 +239,10 @@ def _write_to_anchors_button_clicked(self) -> None: def anchor_postions_updated(self, anchor_positions: dict[int, Position]) -> None: self._data_model.anchor_postions_updated(anchor_positions) + def set_writing(self, writing: bool) -> None: + """Disable the write button while positions are being written""" + self._write_to_anchors_button.setEnabled(not writing) + def _load_button_clicked(self) -> None: names = QFileDialog.getOpenFileName( self, "Open file", self._helper.current_folder, FILE_REGEX_YAML diff --git a/src/cfclient/ui/tabs/locopositioning_tab.py b/src/cfclient/ui/tabs/locopositioning_tab.py index d5f5ff86..6c7bda10 100644 --- a/src/cfclient/ui/tabs/locopositioning_tab.py +++ b/src/cfclient/ui/tabs/locopositioning_tab.py @@ -335,6 +335,9 @@ class LocoPositioningTab(TabToolbox, locopositioning_tab_class): # Frame rate (updates per second) FPS = 2 + # Max difference (m) for an anchor position to count as already written + ANCHOR_POSITION_TOLERANCE = 0.005 + def __init__(self, helper: PluginHelper) -> None: super(LocoPositioningTab, self).__init__(helper, "Loco Positioning") self.setupUi(self) @@ -785,16 +788,48 @@ def _update_positions_in_config_dialog(self) -> None: def write_positions_to_anchors( self, anchor_positions: dict[int, tuple[float, float, float]] ) -> None: - if self._cf is not None: - self._write_positions_task = create_task( - self._write_positions_to_anchors_async(self._cf, anchor_positions) - ) + if self._cf is None or self._write_positions_task is not None: + return + + # Anchors that already report the target position don't need a packet + to_write = { + anchor_id: position + for anchor_id, position in anchor_positions.items() + if not self._anchor_has_position(anchor_id, position) + } + logger.info( + "Writing positions to anchors %s, %d already up to date", + sorted(to_write), + len(anchor_positions) - len(to_write), + ) + if not to_write: + return + + self._anchor_position_dialog.set_writing(True) + self._write_positions_task = create_task( + self._write_positions_to_anchors_async(self._cf, to_write) + ) + + def _anchor_has_position( + self, anchor_id: int, position: tuple[float, float, float] + ) -> bool: + anchor = self._anchors.get(anchor_id) + if anchor is None or not anchor.is_position_valid(): + return False + return all( + abs(current - target) < self.ANCHOR_POSITION_TOLERANCE + for current, target in zip(anchor.get_position(), position) + ) async def _write_positions_to_anchors_async( self, cf: Crazyflie, anchor_positions: dict[int, tuple[float, float, float]] ) -> None: lopo = LoPoAnchor(cf) - for _ in range(3): - for anchor_id, position in anchor_positions.items(): - await lopo.set_position(anchor_id, position) - await asyncio.sleep(0.2) + try: + for _ in range(3): + for anchor_id, position in anchor_positions.items(): + await lopo.set_position(anchor_id, position) + await asyncio.sleep(0.2) + finally: + self._write_positions_task = None + self._anchor_position_dialog.set_writing(False)