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)
