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]
