def calibrate_pressed(self): self.stop() aw = CalibrateWindow(self) aw.setAttribute(Qt.WA_QuitOnClose) aw.show()
class IMU(PluginBase, Ui_IMU): def __init__(self, ipcon, uid, version): PluginBase.__init__(self, ipcon, uid, 'IMU Brick', version) self.setupUi(self) self.imu = BrickIMU(uid, ipcon) self.device = self.imu 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.darkGreen], ["Z", Qt.blue]]) self.acc_plot = Plot("Acceleration [mG]", [["X", Qt.red], ["Y", Qt.darkGreen], ["Z", Qt.blue]]) self.gyr_plot = Plot("Angular Velocity [%c/s]" % 0xB0, [["X", Qt.red], ["Y", Qt.darkGreen], ["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) self.calibrate = None self.alive = True def start(self): if not self.alive: return self.gl_layout.activate() async_call(self.imu.set_all_data_period, 100, None, self.increase_error_count) async_call(self.imu.set_orientation_period, 100, None, self.increase_error_count) async_call(self.imu.set_quaternion_period, 50, None, self.increase_error_count) self.update_timer.start(50) async_call(self.imu.get_convergence_speed, None, self.speed_spinbox.setValue, self.increase_error_count) def stop(self): self.update_timer.stop() async_call(self.imu.set_all_data_period, 0, None, self.increase_error_count) async_call(self.imu.set_orientation_period, 0, None, self.increase_error_count) async_call(self.imu.set_quaternion_period, 0, None, self.increase_error_count) def destroy(self): self.alive = False if self.calibrate: self.calibrate.close() def has_reset_device(self): return self.version >= (1, 0, 7) def reset_device(self): if self.has_reset_device(): self.imu.reset() def is_brick(self): return True 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, 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() 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)