class IMU(PluginBase, Ui_IMU): def __init__(self, *args): PluginBase.__init__(self, BrickIMU, *args) self.setupUi(self) self.imu = self.device self.acc_x = 0 self.acc_y = 0 self.acc_z = 0 self.mag_x = 0 self.mag_y = 0 self.mag_z = 0 self.gyr_x = 0 self.gyr_y = 0 self.gyr_z = 0 self.tem = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.qua_x = 0 self.qua_y = 0 self.qua_z = 0 self.qua_w = 0 self.old_time = 0 self.update_timer = QTimer() self.update_timer.timeout.connect(self.update_data) self.cbe_all_data = CallbackEmulator(self.imu.get_all_data, self.all_data_callback, self.increase_error_count, use_data_signal=False) self.cbe_orientation = CallbackEmulator(self.imu.get_orientation, self.orientation_callback, self.increase_error_count, use_data_signal=False) self.cbe_quaternion = CallbackEmulator(self.imu.get_quaternion, self.quaternion_callback, self.increase_error_count, use_data_signal=False) # Import IMUGLWidget here, not global. If globally included we get # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate' # as soon as IMU is set as device_class in __init__. # No idea why this happens, doesn't make sense. try: from .imu_gl_widget import IMUGLWidget except: from imu_gl_widget import IMUGLWidget self.imu_gl = IMUGLWidget(self) self.imu_gl.setMinimumSize(150, 150) self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.min_x = 0 self.min_y = 0 self.min_z = 0 self.max_x = 0 self.max_y = 0 self.max_z = 0 self.update_counter = 0 self.mag_plot_widget = PlotWidget("Magnetic Field [mG]", [["X", Qt.red, self.get_mag_x], ["Y", Qt.darkGreen, self.get_mag_y], ["Z", Qt.blue, self.get_mag_z]], self.clear_graphs) self.acc_plot_widget = PlotWidget("Acceleration [mG]", [["X", Qt.red, self.get_acc_x], ["Y", Qt.darkGreen, self.get_acc_y], ["Z", Qt.blue, self.get_acc_z]], self.clear_graphs) self.gyr_plot_widget = PlotWidget("Angular Velocity [%c/s]" % 0xB0, [["X", Qt.red, self.get_gyr_x], ["Y", Qt.darkGreen, self.get_gyr_y], ["Z", Qt.blue, self.get_gyr_z]], self.clear_graphs) self.tem_plot_widget = PlotWidget("Temperature [%cC]" % 0xB0, [["t", Qt.red, self.get_tem]], self.clear_graphs) self.mag_plot_widget.setMinimumSize(250, 200) self.acc_plot_widget.setMinimumSize(250, 200) self.gyr_plot_widget.setMinimumSize(250, 200) self.tem_plot_widget.setMinimumSize(250, 200) self.orientation_label = QLabel("""Position your IMU Brick as shown \ in the image above, then press "Save Orientation".""") self.orientation_label.setWordWrap(True) self.orientation_label.setAlignment(Qt.AlignHCenter) self.gl_layout = QVBoxLayout() self.gl_layout.addWidget(self.imu_gl) self.gl_layout.addWidget(self.orientation_label) self.layout_top.addWidget(self.gyr_plot_widget) self.layout_top.addWidget(self.acc_plot_widget) self.layout_top.addWidget(self.mag_plot_widget) self.layout_bottom.addLayout(self.gl_layout) self.layout_bottom.addWidget(self.tem_plot_widget) self.save_orientation.clicked.connect(self.imu_gl.save_orientation) self.calibrate.clicked.connect(self.calibrate_clicked) self.led_button.clicked.connect(self.led_clicked) self.speed_spinbox.editingFinished.connect(self.speed_finished) self.calibrate = None self.alive = True if self.firmware_version >= (1, 0, 7): reset = QAction('Reset', self) reset.triggered.connect(lambda: self.imu.reset()) self.set_actions(reset) def start(self): if not self.alive: return self.gl_layout.activate() self.cbe_all_data.set_period(100) self.cbe_orientation.set_period(100) self.cbe_quaternion.set_period(50) self.update_timer.start(50) async_call(self.imu.get_convergence_speed, None, self.speed_spinbox.setValue, self.increase_error_count) self.mag_plot_widget.stop = False self.acc_plot_widget.stop = False self.gyr_plot_widget.stop = False self.tem_plot_widget.stop = False def stop(self): self.mag_plot_widget.stop = True self.acc_plot_widget.stop = True self.gyr_plot_widget.stop = True self.tem_plot_widget.stop = True self.update_timer.stop() self.cbe_all_data.set_period(0) self.cbe_orientation.set_period(0) self.cbe_quaternion.set_period(0) def destroy(self): self.alive = False if self.calibrate: self.calibrate.close() def get_url_part(self): return 'imu' @staticmethod def has_device_identifier(device_identifier): return device_identifier == BrickIMU.DEVICE_IDENTIFIER def all_data_callback(self, data): acc_x, acc_y, acc_z, mag_x, mag_y, mag_z, gyr_x, gyr_y, gyr_z, tem = data self.acc_x = acc_x self.acc_y = acc_y self.acc_z = acc_z self.mag_x = mag_x self.mag_y = mag_y self.mag_z = mag_z self.gyr_x = gyr_x self.gyr_y = gyr_y self.gyr_z = gyr_z self.tem = tem def quaternion_callback(self, data): qua_x, qua_y, qua_z, qua_w = data self.qua_x = qua_x self.qua_y = qua_y self.qua_z = qua_z self.qua_w = qua_w def orientation_callback(self, data): roll, pitch, yaw = data self.roll = roll self.pitch = pitch self.yaw = yaw def led_clicked(self): if 'On' in self.led_button.text(): self.led_button.setText('Turn LEDs Off') self.imu.leds_on() elif 'Off' in self.led_button.text(): self.led_button.setText('Turn LEDs On') self.imu.leds_off() def get_acc_x(self): return self.acc_x def get_acc_y(self): return self.acc_y def get_acc_z(self): return self.acc_z def get_mag_x(self): return self.mag_x def get_mag_y(self): return self.mag_y def get_mag_z(self): return self.mag_z def get_gyr_x(self): return self.gyr_x/14.375 def get_gyr_y(self): return self.gyr_y/14.375 def get_gyr_z(self): return self.gyr_z/14.375 def get_tem(self): return self.tem/100.0 def update_data(self): self.update_counter += 1 self.imu_gl.update(self.qua_x, self.qua_y, self.qua_z, self.qua_w) if self.update_counter % 2: gyr_x = self.gyr_x/14.375 gyr_y = self.gyr_y/14.375 gyr_z = self.gyr_z/14.375 self.acceleration_update(self.acc_x, self.acc_y, self.acc_z) self.magnetometer_update(self.mag_x, self.mag_y, self.mag_z) self.gyroscope_update(gyr_x, gyr_y, gyr_z) self.orientation_update(self.roll, self.pitch, self.yaw) self.temperature_update(self.tem) def acceleration_update(self, x, y, z): x_str = "%g" % x y_str = "%g" % y z_str = "%g" % z self.acc_y_label.setText(y_str) self.acc_x_label.setText(x_str) self.acc_z_label.setText(z_str) def magnetometer_update(self, x, y, z): # Earth magnetic field. 0.5 Gauss x_str = "%g" % x y_str = "%g" % y z_str = "%g" % z self.mag_x_label.setText(x_str) self.mag_y_label.setText(y_str) self.mag_z_label.setText(z_str) def gyroscope_update(self, x, y, z): x_str = "%g" % int(x) y_str = "%g" % int(y) z_str = "%g" % int(z) self.gyr_x_label.setText(x_str) self.gyr_y_label.setText(y_str) self.gyr_z_label.setText(z_str) def orientation_update(self, r, p, y): r_str = "%g" % (r/100) p_str = "%g" % (p/100) y_str = "%g" % (y/100) self.roll_label.setText(r_str) self.pitch_label.setText(p_str) self.yaw_label.setText(y_str) def temperature_update(self, t): t_str = "%.2f" % (t/100.0) self.tem_label.setText(t_str) def calibrate_clicked(self): self.stop() if self.calibrate is None: self.calibrate = CalibrateWindow(self) self.calibrate.refresh_values() self.calibrate.show() def speed_finished(self): speed = self.speed_spinbox.value() self.imu.set_convergence_speed(speed)
class IMU(PluginBase, Ui_IMU): def __init__(self, ipcon, uid): PluginBase.__init__(self, ipcon, uid) self.setupUi(self) self.imu = brick_imu.IMU(self.uid) self.device = self.imu self.ipcon.add_device(self.imu) version = self.imu.get_version() self.version = ".".join(map(str, version[1])) self.version_minor = version[1][1] self.version_release = version[1][2] self.acc_x = 0 self.acc_y = 0 self.acc_z = 0 self.mag_x = 0 self.mag_y = 0 self.mag_z = 0 self.gyr_x = 0 self.gyr_y = 0 self.gyr_z = 0 self.tem = 0 self.roll = 0 self.pitch = 0 self.yaw = 0 self.qua_x = 0 self.qua_y = 0 self.qua_z = 0 self.qua_w = 0 self.old_time = 0 self.update_timer = QTimer() self.update_timer.timeout.connect(self.update_data) self.imu.register_callback(self.imu.CALLBACK_ALL_DATA, self.all_data_callback) self.imu.register_callback(self.imu.CALLBACK_ORIENTATION, self.orientation_callback) self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_callback) # Import IMUGLWidget here, not global. If globally included we get # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate' # as soon as IMU is set as device_class in __init__. # No idea why this happens, doesn't make sense. from imu_gl_widget import IMUGLWidget self.imu_gl = IMUGLWidget(self) self.imu_gl.setMinimumSize(150, 150) self.imu_gl.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) self.min_x = 0 self.min_y = 0 self.min_z = 0 self.max_x = 0 self.max_y = 0 self.max_z = 0 self.counter = 0 self.update_counter = 0 self.mag_plot = Plot("Magnetic Field [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]]) self.acc_plot = Plot("Acceleration [mG]", [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]]) self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0, [["X", Qt.red], ["Y", Qt.green], ["Z", Qt.blue]]) self.tem_plot = Plot("Temperature [%cC]" % 0xB0, [["t", Qt.red]]) self.mag_plot.setMinimumSize(250, 200) self.acc_plot.setMinimumSize(250, 200) self.gyr_plot.setMinimumSize(250, 200) self.tem_plot.setMinimumSize(250, 200) self.orientation_label = QLabel( """Position your IMU Brick as shown \ in the image above, then press "Save Orientation".""" ) self.orientation_label.setWordWrap(True) self.orientation_label.setAlignment(Qt.AlignHCenter) self.gl_layout = QVBoxLayout() self.gl_layout.addWidget(self.imu_gl) self.gl_layout.addWidget(self.orientation_label) self.layout_top.addWidget(self.gyr_plot) self.layout_top.addWidget(self.acc_plot) self.layout_top.addWidget(self.mag_plot) self.layout_bottom.addLayout(self.gl_layout) self.layout_bottom.addWidget(self.tem_plot) self.save_orientation.clicked.connect(self.imu_gl.save_orientation) self.clear_graphs.clicked.connect(self.clear_graphs_clicked) self.calibrate.clicked.connect(self.calibrate_pressed) self.led_button.clicked.connect(self.led_clicked) self.speed_spinbox.editingFinished.connect(self.speed_finished) def start(self): self.gl_layout.activate() self.imu.set_all_data_period(100) self.imu.set_orientation_period(100) self.imu.set_quaternion_period(50) self.update_timer.start(50) speed = self.imu.get_convergence_speed() self.speed_spinbox.setValue(speed) def stop(self): self.update_timer.stop() self.imu.set_all_data_period(0) self.imu.set_orientation_period(0) self.imu.set_quaternion_period(0) def has_reset_device(self): return self.version_minor > 0 or (self.version_minor == 0 and self.version_release > 6) def reset_device(self): if self.has_reset_device(): self.imu.reset() def get_chip_temperature(self): if self.version_minor > 0 or (self.version_minor == 0 and self.version_release > 6): return u"{0} °C".format(self.imu.get_chip_temperature() / 10.0) else: return "(> 1.0.6 needed)" @staticmethod def has_name(name): return "IMU Brick" in name def all_data_callback(self, acc_x, acc_y, acc_z, mag_x, mag_y, mag_z, gyr_x, gyr_y, gyr_z, tem): self.acc_x = acc_x self.acc_y = acc_y self.acc_z = acc_z self.mag_x = mag_x self.mag_y = mag_y self.mag_z = mag_z self.gyr_x = gyr_x self.gyr_y = gyr_y self.gyr_z = gyr_z self.tem = tem def quaternion_callback(self, qua_x, qua_y, qua_z, qua_w): self.qua_x = qua_x self.qua_y = qua_y self.qua_z = qua_z self.qua_w = qua_w def orientation_callback(self, roll, pitch, yaw): self.roll = roll self.pitch = pitch self.yaw = yaw def clear_graphs_clicked(self): self.counter = 0 self.mag_plot.clear_graph() self.acc_plot.clear_graph() self.gyr_plot.clear_graph() self.tem_plot.clear_graph() def led_clicked(self): if "On" in self.led_button.text(): self.led_button.setText("Turn LEDs Off") self.imu.leds_on() elif "Off" in self.led_button.text(): self.led_button.setText("Turn LEDs On") self.imu.leds_off() def update_data(self): self.update_counter += 1 self.imu_gl.update(self.qua_x, self.qua_y, self.qua_z, self.qua_w) if self.update_counter % 2: gyr_x = self.gyr_x / 14.375 gyr_y = self.gyr_y / 14.375 gyr_z = self.gyr_z / 14.375 self.acceleration_update(self.acc_x, self.acc_y, self.acc_z) self.magnetometer_update(self.mag_x, self.mag_y, self.mag_z) self.gyroscope_update(gyr_x, gyr_y, gyr_z) self.orientation_update(self.roll, self.pitch, self.yaw) self.temperature_update(self.tem) self.gyr_plot.add_data(0, self.counter, gyr_x) self.gyr_plot.add_data(1, self.counter, gyr_y) self.gyr_plot.add_data(2, self.counter, gyr_z) self.acc_plot.add_data(0, self.counter, self.acc_x) self.acc_plot.add_data(1, self.counter, self.acc_y) self.acc_plot.add_data(2, self.counter, self.acc_z) self.mag_plot.add_data(0, self.counter, self.mag_x) self.mag_plot.add_data(1, self.counter, self.mag_y) self.mag_plot.add_data(2, self.counter, self.mag_z) self.tem_plot.add_data(0, self.counter, self.tem / 100.0) self.counter += 0.1 def acceleration_update(self, x, y, z): x_str = "%g" % x y_str = "%g" % y z_str = "%g" % z self.acc_y_label.setText(y_str) self.acc_x_label.setText(x_str) self.acc_z_label.setText(z_str) def magnetometer_update(self, x, y, z): # Earth magnetic field. 0.5 Gauss x_str = "%g" % x y_str = "%g" % y z_str = "%g" % z self.mag_x_label.setText(x_str) self.mag_y_label.setText(y_str) self.mag_z_label.setText(z_str) def gyroscope_update(self, x, y, z): x_str = "%g" % int(x) y_str = "%g" % int(y) z_str = "%g" % int(z) self.gyr_x_label.setText(x_str) self.gyr_y_label.setText(y_str) self.gyr_z_label.setText(z_str) def orientation_update(self, r, p, y): r_str = "%g" % (r / 100) p_str = "%g" % (p / 100) y_str = "%g" % (y / 100) self.roll_label.setText(r_str) self.pitch_label.setText(p_str) self.yaw_label.setText(y_str) def temperature_update(self, t): t_str = "%.2f" % (t / 100.0) self.tem_label.setText(t_str) def calibrate_pressed(self): self.stop() aw = CalibrateWindow(self) aw.setAttribute(Qt.WA_QuitOnClose) aw.show() def speed_finished(self): speed = self.speed_spinbox.value() self.imu.set_convergence_speed(speed)
def set_groups(self, keys, groups, relevant_keys, relevant_items, all_values, uniquepos): """Set the current data groups and update the Group widget """ layout = QVBoxLayout() header_widths = [] header_views = [] palette = self.palette() all_values = all_values.keys() def for_print(rd): attrs = [] for d in rd: attr = Orange.data.ContinuousVariable(next(inactive_name_gen)) attr.attributes.update(d) attrs.append(attr) return Orange.data.Domain(attrs, None) for separatev, domain in [(None, for_print(relevant_items))] + groups: label = None if separatev is not None: ann_vals = " <b>|</b> ".join(["<b>{0}</ b> = {1}".format(key,val) \ for key, val in zip(keys, separatev)]) label = QLabel(ann_vals) model = QStandardItemModel() for i, attr in enumerate(domain.attributes): item = QStandardItem() if separatev is not None: isunique = uniquepos[separatev][i] else: isunique = all(a[i] for a in uniquepos.values()) if str(attr.name).startswith( "!!missing " ): # TODO: Change this to not depend on name header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key not in relevant_items[i]] header_text = "\n".join( header_text) if header_text else "Empty" item.setData(header_text, Qt.DisplayRole) item.setFlags(Qt.NoItemFlags) item.setData(QColor(Qt.red), Qt.ForegroundRole) item.setData( palette.color(QPalette.Disabled, QPalette.Window), Qt.BackgroundRole) item.setData("Missing feature.", Qt.ToolTipRole) elif str(attr.name).startswith("!!inactive "): header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key in relevant_items[i]] header_text = "\n".join( header_text) if header_text else "No descriptor" item.setData(header_text, Qt.DisplayRole) item.setData( palette.color(QPalette.Disabled, QPalette.Window), Qt.BackgroundRole) else: header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key not in relevant_items[i]] header_text = "\n".join( header_text) if header_text else "Empty" item.setData(header_text, Qt.DisplayRole) item.setData(attr.name, Qt.ToolTipRole) if not isunique: item.setData(QColor(Qt.red), Qt.ForegroundRole) model.setHorizontalHeaderItem(i, item) attr_count = len(domain.attributes) view = MyHeaderView(Qt.Horizontal) view.setResizeMode(QHeaderView.Fixed) view.setModel(model) hint = view.sizeHint() view.setMaximumHeight(hint.height()) widths = [view.sectionSizeHint(i) for i in range(attr_count)] header_widths.append(widths) header_views.append(view) if label: layout.addWidget(label) layout.addWidget(view) layout.addSpacing(8) # Make all header sections the same width width_sum = 0 max_header_count = max([h.count() for h in header_views]) for i in range(max_header_count): max_width = max([w[i] for w in header_widths if i < len(w)] or [0]) for view in header_views: if i < view.count(): view.resizeSection(i, max_width) width_sum += max_width + 2 for h in header_views: h.setMinimumWidth(h.length() + 4) widget = QWidget() widget.setLayout(layout) widget.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.Maximum) layout.activate() max_width = max(h.length() for h in header_views) + 20 left, _, right, _ = self.getContentsMargins() widget.setMinimumWidth(width_sum) widget.setMinimumWidth(max_width + left + right) self.groups_scroll_area.setWidget(widget)
def set_groups(self, keys, groups, relevant_keys, relevant_items, all_values, uniquepos): """Set the current data groups and update the Group widget """ layout = QVBoxLayout() header_widths = [] header_views = [] palette = self.palette() all_values = all_values.keys() def for_print(rd): attrs = [] for d in rd: attr = Orange.data.ContinuousVariable(next(inactive_name_gen)) attr.attributes.update(d) attrs.append(attr) return Orange.data.Domain(attrs, None) for separatev, domain in [(None, for_print(relevant_items))] + groups: label = None if separatev is not None: ann_vals = " <b>|</b> ".join(["<b>{0}</ b> = {1}".format(key,val) \ for key, val in zip(keys, separatev)]) label = QLabel(ann_vals) model = QStandardItemModel() for i, attr in enumerate(domain.attributes): item = QStandardItem() if separatev is not None: isunique = uniquepos[separatev][i] else: isunique = all(a[i] for a in uniquepos.values()) if str(attr.name).startswith("!!missing "): # TODO: Change this to not depend on name header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key not in relevant_items[i]] header_text = "\n".join(header_text) if header_text else "Empty" item.setData(header_text, Qt.DisplayRole) item.setFlags(Qt.NoItemFlags) item.setData(QColor(Qt.red), Qt.ForegroundRole) item.setData(palette.color(QPalette.Disabled, QPalette.Window), Qt.BackgroundRole) item.setData("Missing feature.", Qt.ToolTipRole) elif str(attr.name).startswith("!!inactive "): header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key in relevant_items[i]] header_text = "\n".join(header_text) if header_text else "No descriptor" item.setData(header_text, Qt.DisplayRole) item.setData(palette.color(QPalette.Disabled, QPalette.Window), Qt.BackgroundRole) else: header_text = ["{0}={1}".format(key, attr.attributes.get(key, "?")) \ for key in all_values if key not in relevant_items[i]] header_text = "\n".join(header_text) if header_text else "Empty" item.setData(header_text, Qt.DisplayRole) item.setData(attr.name, Qt.ToolTipRole) if not isunique: item.setData(QColor(Qt.red), Qt.ForegroundRole) model.setHorizontalHeaderItem(i, item) attr_count = len(domain.attributes) view = MyHeaderView(Qt.Horizontal) view.setResizeMode(QHeaderView.Fixed) view.setModel(model) hint = view.sizeHint() view.setMaximumHeight(hint.height()) widths = [view.sectionSizeHint(i) for i in range(attr_count)] header_widths.append(widths) header_views.append(view) if label: layout.addWidget(label) layout.addWidget(view) layout.addSpacing(8) # Make all header sections the same width width_sum = 0 max_header_count = max([h.count() for h in header_views]) for i in range(max_header_count): max_width = max([w[i] for w in header_widths if i < len(w)] or [0]) for view in header_views: if i < view.count(): view.resizeSection(i, max_width) width_sum += max_width + 2 for h in header_views: h.setMinimumWidth(h.length() + 4) widget = QWidget() widget.setLayout(layout) widget.setSizePolicy(QSizePolicy.MinimumExpanding, QSizePolicy.Maximum) layout.activate() max_width = max(h.length() for h in header_views) + 20 left, _, right, _ = self.getContentsMargins() widget.setMinimumWidth(width_sum) widget.setMinimumWidth(max_width + left + right) self.groups_scroll_area.setWidget(widget)
class IMUV2(PluginBase, Ui_IMUV2): def __init__(self, *args): PluginBase.__init__(self, BrickIMUV2, *args) self.setupUi(self) self.imu = self.device self.cbe_all_data = CallbackEmulator(self.imu.get_all_data, self.all_data_callback, self.increase_error_count) # Import IMUGLWidget here, not global. If globally included we get # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate' # as soon as IMU is set as device_class in __init__. # No idea why this happens, doesn't make sense. try: from .imu_v2_gl_widget import IMUV2GLWidget except: from imu_v2_gl_widget import IMUV2GLWidget self.imu_gl = IMUV2GLWidget(self) self.imu_gl.setFixedSize(200, 200) self.imu_gl_wrapper = None self.data_plot_widget = [] self.sensor_data = [0]*23 self.data_labels = [self.label_acceleration_x, self.label_acceleration_y, self.label_acceleration_z, self.label_magnetic_field_x, self.label_magnetic_field_y, self.label_magnetic_field_z, self.label_angular_velocity_x, self.label_angular_velocity_y, self.label_angular_velocity_z, self.label_euler_angle_heading, self.label_euler_angle_roll, self.label_euler_angle_pitch, self.label_quaternion_w, self.label_quaternion_x, self.label_quaternion_y, self.label_quaternion_z, self.label_linear_acceleration_x, self.label_linear_acceleration_y, self.label_linear_acceleration_z, self.label_gravity_vector_x, self.label_gravity_vector_y, self.label_gravity_vector_z, self.label_temperature] self.data_rows = [[self.label_acceleration_11, self.label_acceleration_21, self.label_acceleration_22, self.label_acceleration_23, self.label_acceleration_41, self.label_acceleration_42, self.label_acceleration_43, self.label_acceleration_x, self.label_acceleration_y, self.label_acceleration_z], [self.label_magnetic_field_11, self.label_magnetic_field_21, self.label_magnetic_field_22, self.label_magnetic_field_23, self.label_magnetic_field_41, self.label_magnetic_field_42, self.label_magnetic_field_43, self.label_magnetic_field_x, self.label_magnetic_field_y, self.label_magnetic_field_z], [self.label_angular_velocity_11, self.label_angular_velocity_21, self.label_angular_velocity_22, self.label_angular_velocity_23, self.label_angular_velocity_41, self.label_angular_velocity_42, self.label_angular_velocity_43, self.label_angular_velocity_x, self.label_angular_velocity_y, self.label_angular_velocity_z], [self.label_euler_angle_11, self.label_euler_angle_21, self.label_euler_angle_22, self.label_euler_angle_23, self.label_euler_angle_41, self.label_euler_angle_42, self.label_euler_angle_43, self.label_euler_angle_roll, self.label_euler_angle_pitch, self.label_euler_angle_heading], [self.label_quaternion_11, self.label_quaternion_21, self.label_quaternion_22, self.label_quaternion_23, self.label_quaternion_24, self.label_quaternion_41, self.label_quaternion_42, self.label_quaternion_43, self.label_quaternion_44, self.label_quaternion_w, self.label_quaternion_x, self.label_quaternion_y, self.label_quaternion_z], [self.label_linear_acceleration_11, self.label_linear_acceleration_21, self.label_linear_acceleration_22, self.label_linear_acceleration_23, self.label_linear_acceleration_41, self.label_linear_acceleration_42, self.label_linear_acceleration_43, self.label_linear_acceleration_x, self.label_linear_acceleration_y, self.label_linear_acceleration_z], [self.label_gravity_vector_11, self.label_gravity_vector_21, self.label_gravity_vector_22, self.label_gravity_vector_23, self.label_gravity_vector_41, self.label_gravity_vector_42, self.label_gravity_vector_43, self.label_gravity_vector_x, self.label_gravity_vector_y, self.label_gravity_vector_z], [self.label_temperature_11, self.label_temperature_21, self.label_temperature_41, self.label_temperature]] even_color = QColor(240, 240, 240) odd_color = QColor(255, 255, 255) self.data_color = [(Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.magenta, even_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.magenta, odd_color)] even_palette = QPalette() even_palette.setColor(QPalette.Window, even_color) odd_palette = QPalette() odd_palette.setColor(QPalette.Window, odd_color) for i, row in enumerate(self.data_rows): for label in row: if i % 2: label.setPalette(odd_palette) else: label.setPalette(even_palette) label.setAutoFillBackground(True) def get_lambda_data_getter(i): return lambda: self.sensor_data[i] self.plot_timer = QTimer(self) self.plot_timer.start(100) for i in range(23): self.data_plot_widget.append(PlotWidget("", [("", self.data_color[i][0], get_lambda_data_getter(i), str)], self.clear_graphs, scales_visible=False, curve_outer_border_visible=False, curve_motion_granularity=1, canvas_color=self.data_color[i][1], external_timer=self.plot_timer, curve_start='right', key=None)) for w in self.data_plot_widget: w.setMinimumHeight(15) w.setMaximumHeight(25) for i in range(23): self.data_grid.addWidget(self.data_plot_widget[i], i, 4) self.data_grid.setColumnMinimumWidth(2, 75) self.gl_layout = QVBoxLayout() self.gl_layout.addWidget(self.imu_gl) self.v_layout = QVBoxLayout() self.layout_bottom.addLayout(self.gl_layout) self.save_orientation.clicked.connect(self.imu_gl.save_orientation) self.checkbox_leds.stateChanged.connect(self.led_clicked) self.button_calibration.clicked.connect(self.calibration_clicked) self.button_detach_3d_view.clicked.connect(self.detach_3d_view_clicked) self.calibration_color = [Qt.red, QColor(0xFF, 0xA0, 0x00), Qt.yellow, Qt.darkGreen] self.calibration = None self.alive = True self.callback_counter = 0 reset = QAction('Reset', self) reset.triggered.connect(lambda: self.imu.reset()) self.set_actions(reset) def start(self): if not self.alive: return self.gl_layout.activate() self.cbe_all_data.set_period(50) for w in self.data_plot_widget: w.stop = False def stop(self): for w in self.data_plot_widget: w.stop = True if self.imu_gl_wrapper == None: self.cbe_all_data.set_period(0) def destroy(self): self.alive = False if self.calibration: self.calibration.close() if self.imu_gl_wrapper: self.imu_gl_wrapper.close() def get_url_part(self): return 'imu_v2' @staticmethod def has_device_identifier(device_identifier): return device_identifier == BrickIMUV2.DEVICE_IDENTIFIER def calibration_clicked(self): if self.calibration is None: self.calibration = Calibration(self) self.button_calibration.setEnabled(False) self.calibration.show() def detach_3d_view_clicked(self): if self.imu_gl_wrapper != None: self.imu_gl_wrapper.close() self.button_detach_3d_view.setEnabled(False) self.imu_gl_wrapper = WrapperWidget(self) self.gl_layout.removeWidget(self.imu_gl) self.imu_gl.setParent(None) self.imu_gl.setMinimumSize(0, 0) self.imu_gl.setMaximumSize(16777215, 16777215) self.imu_gl_wrapper.layout().addWidget(self.imu_gl) self.imu_gl_wrapper.show() def all_data_callback(self, data): self.callback_counter += 1 if self.callback_counter % 2 == 0: self.sensor_data[0] = data.acceleration[0]/100.0 self.sensor_data[1] = data.acceleration[1]/100.0 self.sensor_data[2] = data.acceleration[2]/100.0 self.sensor_data[3] = data.magnetic_field[0]/16.0 self.sensor_data[4] = data.magnetic_field[1]/16.0 self.sensor_data[5] = data.magnetic_field[2]/16.0 self.sensor_data[6] = data.angular_velocity[0]/16.0 self.sensor_data[7] = data.angular_velocity[1]/16.0 self.sensor_data[8] = data.angular_velocity[2]/16.0 self.sensor_data[9] = data.euler_angle[0]/16.0 self.sensor_data[10] = data.euler_angle[1]/16.0 self.sensor_data[11] = data.euler_angle[2]/16.0 self.sensor_data[12] = data.quaternion[0]/(float(2**14-1)) self.sensor_data[13] = data.quaternion[1]/(float(2**14-1)) self.sensor_data[14] = data.quaternion[2]/(float(2**14-1)) self.sensor_data[15] = data.quaternion[3]/(float(2**14-1)) self.sensor_data[16] = data.linear_acceleration[0]/100.0 self.sensor_data[17] = data.linear_acceleration[1]/100.0 self.sensor_data[18] = data.linear_acceleration[2]/100.0 self.sensor_data[19] = data.gravity_vector[0]/100.0 self.sensor_data[20] = data.gravity_vector[1]/100.0 self.sensor_data[21] = data.gravity_vector[2]/100.0 self.sensor_data[22] = data.temperature for i in range(23): self.data_labels[i].setText("{0:.2f}".format(self.sensor_data[i])) self.imu_gl.update(self.sensor_data[12], self.sensor_data[13], self.sensor_data[14], self.sensor_data[15]) cal_mag = data.calibration_status & 3; cal_acc = (data.calibration_status & (3 << 2)) >> 2 cal_gyr = (data.calibration_status & (3 << 4)) >> 4 cal_sys = (data.calibration_status & (3 << 6)) >> 6 if self.calibration != None: self.calibration.save_calibration.setEnabled(data.calibration_status == 0xFF) self.calibration.mag_color.set_color(self.calibration_color[cal_mag]) self.calibration.acc_color.set_color(self.calibration_color[cal_acc]) self.calibration.gyr_color.set_color(self.calibration_color[cal_gyr]) self.calibration.sys_color.set_color(self.calibration_color[cal_sys]) else: self.imu_gl.update(data.quaternion[0]/(float(2**14-1)), data.quaternion[1]/(float(2**14-1)), data.quaternion[2]/(float(2**14-1)), data.quaternion[3]/(float(2**14-1))) def led_clicked(self, state): if state == Qt.Checked: self.imu.leds_on() else: self.imu.leds_off()
class IMUV2(PluginBase, Ui_IMUV2): def __init__(self, *args): PluginBase.__init__(self, BrickIMUV2, *args) self.setupUi(self) self.imu = self.device self.cbe_all_data = CallbackEmulator(self.imu.get_all_data, self.all_data_callback, self.increase_error_count) # Import IMUGLWidget here, not global. If globally included we get # 'No OpenGL_accelerate module loaded: No module named OpenGL_accelerate' # as soon as IMU is set as device_class in __init__. # No idea why this happens, doesn't make sense. try: from .imu_v2_gl_widget import IMUV2GLWidget except: from imu_v2_gl_widget import IMUV2GLWidget self.imu_gl = IMUV2GLWidget(self) self.imu_gl.setFixedSize(200, 200) self.min_x = 0 self.min_y = 0 self.min_z = 0 self.max_x = 0 self.max_y = 0 self.max_z = 0 self.data_plot_widget = [] self.sensor_data = [0]*23 self.data_labels = [self.label_acceleration_x, self.label_acceleration_y, self.label_acceleration_z, self.label_magnetic_field_x, self.label_magnetic_field_y, self.label_magnetic_field_z, self.label_angular_velocity_x, self.label_angular_velocity_y, self.label_angular_velocity_z, self.label_euler_angle_heading, self.label_euler_angle_roll, self.label_euler_angle_pitch, self.label_quaternion_w, self.label_quaternion_x, self.label_quaternion_y, self.label_quaternion_z, self.label_linear_acceleration_x, self.label_linear_acceleration_y, self.label_linear_acceleration_z, self.label_gravity_vector_x, self.label_gravity_vector_y, self.label_gravity_vector_z, self.label_temperature] self.data_rows = [[self.label_acceleration_11, self.label_acceleration_21, self.label_acceleration_22, self.label_acceleration_23, self.label_acceleration_41, self.label_acceleration_42, self.label_acceleration_43, self.label_acceleration_x, self.label_acceleration_y, self.label_acceleration_z], [self.label_magnetic_field_11, self.label_magnetic_field_21, self.label_magnetic_field_22, self.label_magnetic_field_23, self.label_magnetic_field_41, self.label_magnetic_field_42, self.label_magnetic_field_43, self.label_magnetic_field_x, self.label_magnetic_field_y, self.label_magnetic_field_z], [self.label_angular_velocity_11, self.label_angular_velocity_21, self.label_angular_velocity_22, self.label_angular_velocity_23, self.label_angular_velocity_41, self.label_angular_velocity_42, self.label_angular_velocity_43, self.label_angular_velocity_x, self.label_angular_velocity_y, self.label_angular_velocity_z], [self.label_euler_angle_11, self.label_euler_angle_21, self.label_euler_angle_22, self.label_euler_angle_23, self.label_euler_angle_41, self.label_euler_angle_42, self.label_euler_angle_43, self.label_euler_angle_roll, self.label_euler_angle_pitch, self.label_euler_angle_heading], [self.label_quaternion_11, self.label_quaternion_21, self.label_quaternion_22, self.label_quaternion_23, self.label_quaternion_24, self.label_quaternion_41, self.label_quaternion_42, self.label_quaternion_43, self.label_quaternion_44, self.label_quaternion_w, self.label_quaternion_x, self.label_quaternion_y, self.label_quaternion_z], [self.label_linear_acceleration_11, self.label_linear_acceleration_21, self.label_linear_acceleration_22, self.label_linear_acceleration_23, self.label_linear_acceleration_41, self.label_linear_acceleration_42, self.label_linear_acceleration_43, self.label_linear_acceleration_x, self.label_linear_acceleration_y, self.label_linear_acceleration_z], [self.label_gravity_vector_11, self.label_gravity_vector_21, self.label_gravity_vector_22, self.label_gravity_vector_23, self.label_gravity_vector_41, self.label_gravity_vector_42, self.label_gravity_vector_43, self.label_gravity_vector_x, self.label_gravity_vector_y, self.label_gravity_vector_z], [self.label_temperature_11, self.label_temperature_21, self.label_temperature_41, self.label_temperature], ] even_color = QColor(240, 240, 240) odd_color = QColor(255, 255, 255) self.data_color = [(Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.magenta, even_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.red, odd_color), (Qt.darkGreen, odd_color), (Qt.blue, odd_color), (Qt.red, even_color), (Qt.darkGreen, even_color), (Qt.blue, even_color), (Qt.magenta, odd_color)] even_palette = QPalette() even_palette.setColor(QPalette.Window, even_color) odd_palette = QPalette() odd_palette.setColor(QPalette.Window, odd_color) for i, row in enumerate(self.data_rows): for label in row: if i % 2: label.setPalette(odd_palette) else: label.setPalette(even_palette) label.setAutoFillBackground(True) def get_lambda_data_getter(i): return lambda: self.get_data(i) self.plot_timer = QTimer(self) self.plot_timer.start(100) for i in range(23): self.data_plot_widget.append(PlotWidget("", [["", self.data_color[i][0], get_lambda_data_getter(i)]], self.clear_graphs, scales_visible=False, curve_outer_border_visible=False, curve_motion_granularity=1, canvas_color=self.data_color[i][1], external_timer=self.plot_timer)) for w in self.data_plot_widget: w.setMinimumHeight(15) w.setMaximumHeight(25) for i in range(23): self.data_grid.addWidget(self.data_plot_widget[i], i, 4) self.data_grid.setColumnMinimumWidth(2, 75) self.gl_layout = QVBoxLayout() self.gl_layout.addWidget(self.imu_gl) self.v_layout = QVBoxLayout() self.layout_bottom.addLayout(self.gl_layout) self.save_orientation.clicked.connect(self.imu_gl.save_orientation) self.checkbox_leds.stateChanged.connect(self.led_clicked) self.button_calibration.clicked.connect(self.calibration_clicked) self.calibration_color = [Qt.red, QColor(0xFF, 0xA0, 0x00), Qt.yellow, Qt.darkGreen] self.calibration = None self.alive = True self.callback_counter = 0 reset = QAction('Reset', self) reset.triggered.connect(lambda: self.imu.reset()) self.set_actions(reset) def start(self): if not self.alive: return self.gl_layout.activate() self.cbe_all_data.set_period(50) for w in self.data_plot_widget: w.stop = False def stop(self): for w in self.data_plot_widget: w.stop = True self.cbe_all_data.set_period(0) def destroy(self): self.alive = False if self.calibration: self.calibration.close() def get_url_part(self): return 'imu_v2' @staticmethod def has_device_identifier(device_identifier): return device_identifier == BrickIMUV2.DEVICE_IDENTIFIER def calibration_clicked(self): if self.calibration is None: self.calibration = Calibration(self) self.button_calibration.setEnabled(False) self.calibration.show() def all_data_callback(self, data): self.callback_counter += 1 if self.callback_counter % 2 == 0: self.sensor_data[0] = data.acceleration[0]/100.0 self.sensor_data[1] = data.acceleration[1]/100.0 self.sensor_data[2] = data.acceleration[2]/100.0 self.sensor_data[3] = data.magnetic_field[0]/16.0 self.sensor_data[4] = data.magnetic_field[1]/16.0 self.sensor_data[5] = data.magnetic_field[2]/16.0 self.sensor_data[6] = data.angular_velocity[0]/16.0 self.sensor_data[7] = data.angular_velocity[1]/16.0 self.sensor_data[8] = data.angular_velocity[2]/16.0 self.sensor_data[9] = data.euler_angle[0]/16.0 self.sensor_data[10] = data.euler_angle[1]/16.0 self.sensor_data[11] = data.euler_angle[2]/16.0 self.sensor_data[12] = data.quaternion[0]/(float(2**14-1)) self.sensor_data[13] = data.quaternion[1]/(float(2**14-1)) self.sensor_data[14] = data.quaternion[2]/(float(2**14-1)) self.sensor_data[15] = data.quaternion[3]/(float(2**14-1)) self.sensor_data[16] = data.linear_acceleration[0]/100.0 self.sensor_data[17] = data.linear_acceleration[1]/100.0 self.sensor_data[18] = data.linear_acceleration[2]/100.0 self.sensor_data[19] = data.gravity_vector[0]/100.0 self.sensor_data[20] = data.gravity_vector[1]/100.0 self.sensor_data[21] = data.gravity_vector[2]/100.0 self.sensor_data[22] = data.temperature for i in range(23): self.data_labels[i].setText("{0:.2f}".format(self.sensor_data[i])) self.imu_gl.update(self.sensor_data[12], self.sensor_data[13], self.sensor_data[14], self.sensor_data[15]) cal_mag = data.calibration_status & 3; cal_acc = (data.calibration_status & (3 << 2)) >> 2 cal_gyr = (data.calibration_status & (3 << 4)) >> 4 cal_sys = (data.calibration_status & (3 << 6)) >> 6 if self.calibration != None: self.calibration.save_calibration.setEnabled(data.calibration_status == 0xFF) self.calibration.mag_color.set_color(self.calibration_color[cal_mag]) self.calibration.acc_color.set_color(self.calibration_color[cal_acc]) self.calibration.gyr_color.set_color(self.calibration_color[cal_gyr]) self.calibration.sys_color.set_color(self.calibration_color[cal_sys]) else: self.imu_gl.update(data.quaternion[0]/(float(2**14-1)), data.quaternion[1]/(float(2**14-1)), data.quaternion[2]/(float(2**14-1)), data.quaternion[3]/(float(2**14-1))) def led_clicked(self, state): if state == Qt.Checked: self.imu.leds_on() else: self.imu.leds_off() def get_data(self, i): return self.sensor_data[i]