diff --git a/src/cfclient/ui/tabs/locopositioning_tab.py b/src/cfclient/ui/tabs/locopositioning_tab.py index fb23ab3a09..b5885f27b5 100644 --- a/src/cfclient/ui/tabs/locopositioning_tab.py +++ b/src/cfclient/ui/tabs/locopositioning_tab.py @@ -32,6 +32,7 @@ import logging from enum import Enum +from collections import namedtuple from PyQt5 import uic from PyQt5.QtCore import pyqtSignal, QTimer @@ -46,6 +47,7 @@ from lpslib.lopoanchor import LoPoAnchor import copy +import sys __author__ = 'Bitcraze AB' __all__ = ['LocoPositioningTab'] @@ -125,7 +127,8 @@ def __init__(self, title, horizontal, vertical): self._vertical = vertical self._depth = self._find_missing_axis(horizontal, vertical) self._title = title - self.widget = pg.PlotWidget(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') @@ -235,6 +238,9 @@ class DisplayMode(Enum): estimated_position = 2 +Range = namedtuple('Range', ['min', 'max']) + + class AnchorPosWrapper(): """Wraps the UI elements of one anchor position""" def __init__(self, x, y, z): @@ -327,6 +333,8 @@ def __init__(self, tabWidget, helper, *args): self._read_positions_from_anchors() ) + self._show_all_button.clicked.connect(self._scale_and_center_graphs) + # Connect the Crazyflie API callbacks to the signals self._helper.cf.connected.add_callback( self._connected_signal.emit) @@ -413,6 +421,109 @@ 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 + + result["x"] = Range(min(ranges["x"].min, point[0]), + max(ranges["x"].max, point[0])) + + result["y"] = Range(min(ranges["y"].min, point[1]), + max(ranges["y"].max, point[1])) + + result["z"] = Range(min(ranges["z"].min, point[2]), + max(ranges["z"].max, point[2])) + + return result + def _connected(self, link_uri): """Callback when the Crazyflie has been connected""" logger.debug("Crazyflie connected to {}".format(link_uri)) diff --git a/src/cfclient/ui/tabs/locopositioning_tab.ui b/src/cfclient/ui/tabs/locopositioning_tab.ui index 937b524b42..9c9fe234af 100644 --- a/src/cfclient/ui/tabs/locopositioning_tab.ui +++ b/src/cfclient/ui/tabs/locopositioning_tab.ui @@ -278,6 +278,39 @@ + + + + Graph settings + + + + + + + + Show all + + + + + + + Qt::Horizontal + + + + 40 + 20 + + + + + + + + +