class Q: HOST = "localhost" PORT = 4223 UID = "aeoUQwwyAvY" # Change to your UID def __init__(self): self.base_x = 0.0 self.base_y = 0.0 self.base_z = 0.0 self.base_w = 0.0 self.imu = IMU(self.UID) # Create device object self.ipcon = IPConnection(self.HOST, self.PORT) # Create IPconnection to brickd self.ipcon.add_device(self.imu) # Add device to IP connection # Don't use device before it is added to a connection # Wait for IMU to settle print 'Set IMU to base position and wait for 10 seconds' print 'Base position will be 0 for all angles' time.sleep(10) q = self.imu.get_quaternion() self.set_base_coordinates(q.x, q.y, q.z, q.w) # Set period for quaternion callback to 10ms self.imu.set_quaternion_period(10) # Register quaternion callback self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_cb) def quaternion_cb(self, x, y, z, w): # Use conjugate of quaternion to rotate coordinates according to base system x, y, z, w = self.make_relative_coordinates(-x, -y, -z, w) x_angle = int(math.atan2(2.0*(y*z - w*x), 1.0 - 2.0*(x*x + y*y))*180/math.pi) y_angle = int(math.atan2(2.0*(x*z + w*y), 1.0 - 2.0*(x*x + y*y))*180/math.pi) z_angle = int(math.atan2(2.0*(x*y + w*z), 1.0 - 2.0*(x*x + z*z))*180/math.pi) print 'x: {0}, y: {1}, z: {2}'.format(x_angle, y_angle, z_angle) def set_base_coordinates(self, x, y, z, w): self.base_x = x self.base_y = y self.base_z = z self.base_w = w def make_relative_coordinates(self, x, y, z, w): # Multiply base quaternion with current quaternion return ( w * self.base_x + x * self.base_w + y * self.base_z - z * self.base_y, w * self.base_y - x * self.base_z + y * self.base_w + z * self.base_x, w * self.base_z + x * self.base_y - y * self.base_x + z * self.base_w, w * self.base_w - x * self.base_x - y * self.base_y - z * self.base_z )
def __init__(self,host,port,uid,callbackPeriodMS=100): self.host=host self.port=port self.uid=uid self._imu = IMU(uid) # Create device object self._ipcon = IPConnection(self.host,self.port) # Create IPconnection to brickd self._ipcon.add_device(self._imu) # Add device to IP connection self.ready = True # Don't use device before it is added to a connection # Set period for quaternion callback (defaults to 100ms) self._imu.set_quaternion_period(callbackPeriodMS) # Register quaternion callback self._imu.register_callback(self._imu.CALLBACK_QUATERNION, self._QuaternionCallback) self._imu.leds_off() # Turn LEDs off. self._imu.set_convergence_speed(5) # 5ms convergence. # Orientation origin and most recent values q = self._imu.get_quaternion() # Get a temp quaternion from current pose. self.rel_x = q.x self.rel_y = q.y self.rel_z = q.z self.rel_w = q.w self.x = q.x self.y = q.y self.z = q.z self.w = q.w
def __init__(self, parent=None): QMainWindow.__init__(self, parent) self.initialized = False self.altitude_error_i = 0 self.acc_scale = 0.0 self.start_time = time.time() self.altitude_error = 0 self.inst_acceleration = 0.0 self.delta = 0 self.estimated_velocity = 0.0 self.estimated_altitude = 0.0 self.last_time = time.time() self.last_orig_altitude = 0 self.last_estimated_altitude = 0 ipcon = IPConnection() self.imu = IMU(UID_IMU, ipcon) self.barometer = Barometer(UID_BAROMETER, ipcon) ipcon.connect(HOST, PORT) # Turn leds and orientation calculation off, to save calculation time # for the IMU Brick. This makes sure that the measurements are taken # in equidistant 2ms intervals self.imu.leds_off() self.imu.orientation_calculation_off() # Turn averaging of in the Barometer Bricklet to make sure that # the data is without delay self.barometer.set_averaging(0, 0, 0) red_pen = QPen(Qt.red) red_pen.setWidth(5) plot_list = [ #['', Qt.blue, self.get_orig_value], ['', red_pen, self.get_estimated_value] ] self.plot_widget = PlotWidget('Height [m]', plot_list) self.plot_widget.stop = False self.setCentralWidget(self.plot_widget) self.timer = QTimer() self.timer.timeout.connect(self.update) self.timer.start(6)
def cb_enumerate(uid, connected_uid, position, hardware_version, firmware_version, device_identifier, enumeration_type): if connected_uid == '0': print 'Master Brick UID: \t%s' % (uid) #print 'UID: %s' % uid #print 'connected_uid: %s' % connected_uid #print 'position: %s' % position #print 'hardware_version: %i.%i.%i' % hardware_version #print 'firmware_version: %i.%i.%i' % firmware_version #print 'device_identifier: %d' % device_identifier #print 'enumeration_type: %d' % enumeration_type if enumeration_type == IPConnection.ENUMERATION_TYPE_CONNECTED or \ enumeration_type == IPConnection.ENUMERATION_TYPE_AVAILABLE: # Enumeration is for IMU Brick if device_identifier == IMU.DEVICE_IDENTIFIER: print 'IMU Brick UID: \t\t%s' % (uid) # Create imu device object imu = IMU(uid, ipcon) imu.set_all_data_period(100) imu.register_callback(imu.CALLBACK_ALL_DATA, cb_imudynamic) # Enumeration is for GPS Bricklet if device_identifier == GPS.DEVICE_IDENTIFIER: print 'GPS Bricklet UID: \t%s' % (uid) # Create imu device object gps = GPS(uid, ipcon) gps.set_coordinates_callback_period(100) gps.register_callback(gps.CALLBACK_COORDINATES, cb_coordinates)
def __init__(self, parent=None): QMainWindow.__init__(self, parent) self.initialized = False self.altitude_error_i = 0 self.acc_scale = 0.0 self.start_time = time.time() self.altitude_error = 0 self.inst_acceleration = 0.0 self.delta = 0 self.estimated_velocity = 0.0 self.estimated_altitude = 0.0 self.last_time = time.time() self.last_orig_altitude = 0 self.last_estimated_altitude = 0 ipcon = IPConnection() self.imu = IMU(UID_IMU, ipcon) self.barometer = Barometer(UID_BAROMETER, ipcon) ipcon.connect(HOST, PORT) # Turn leds and orientation calculation off, to save calculation time # for the IMU Brick. This makes sure that the measurements are taken # in equidistant 2ms intervals self.imu.leds_off() self.imu.orientation_calculation_off() # Turn averaging of in the Barometer Bricklet to make sure that # the data is without delay self.barometer.set_averaging(0, 0, 0) red_pen = QPen(Qt.red) red_pen.setWidth(5) plot_list = [#['', Qt.blue, self.get_orig_value], ['', red_pen, self.get_estimated_value] ] self.plot_widget = PlotWidget('Height [m]', plot_list) self.plot_widget.stop = False self.setCentralWidget(self.plot_widget) self.timer = QTimer() self.timer.timeout.connect(self.update) self.timer.start(6)
def __init__(self): self.base_x = 0.0 self.base_y = 0.0 self.base_z = 0.0 self.base_w = 0.0 self.imu = IMU(self.UID) # Create device object self.ipcon = IPConnection(self.HOST, self.PORT) # Create IPconnection to brickd self.ipcon.add_device(self.imu) # Add device to IP connection # Don't use device before it is added to a connection # Wait for IMU to settle print 'Set IMU to base position and wait for 10 seconds' print 'Base position will be 0 for all angles' time.sleep(10) q = self.imu.get_quaternion() self.set_base_coordinates(q.x, q.y, q.z, q.w) # Set period for quaternion callback to 10ms self.imu.set_quaternion_period(10) # Register quaternion callback self.imu.register_callback(self.imu.CALLBACK_QUATERNION, self.quaternion_cb)
HOST = "localhost" PORT = 4223 UID = "ayQskyoNrCW" # Change to your UID from tinkerforge.ip_connection import IPConnection from tinkerforge.brick_imu import IMU # Quaternion callback def cb_quaternion(x, y, z, w): print("x: " + str(x) + "\ny: " + str(y) + "\nz: " + str(z) + "\nw: " + str(w) + "\n") if __name__ == "__main__": ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Set period for quaternion callback to 1s imu.set_quaternion_period(1000) # Register quaternion callback imu.register_callback(imu.CALLBACK_QUATERNION, cb_quaternion) raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.disconnect()
#!/usr/bin/env python # -*- coding: utf-8 -*- HOST = "localhost" PORT = 4223 UID = "a4JritAp6Go" # Change to your UID from tinkerforge.ip_connection import IPConnection from tinkerforge.brick_imu import IMU imu = IMU(UID) # Create device object # Quaternion callback def quaternion_cb(x, y, z, w): print("x: " + str(x) + "\ny: " + str(y) + "\nz: " + str(z) + "\nw: " + str(w) + "\n") if __name__ == "__main__": ipcon = IPConnection(HOST, PORT) # Create IPconnection to brickd ipcon.add_device(imu) # Add device to IP connection # Don't use device before it is added to a connection # Set period for quaternion callback to 1s imu.set_quaternion_period(100) # Register quaternion callback imu.register_callback(imu.CALLBACK_QUATERNION, quaternion_cb) imu.leds_off() # Turn LEDs off. raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.destroy()
pitchrate = ang_y / 14.375 * 3.14 / 180.0 yawrate = ang_z / 14.375 * 3.14 / 180.0 temp = temp / 100.0 collect(ax, ay, az, rollrate, pitchrate, yawrate, temp) # In[ ]: # In[10]: if __name__ == "__main__": ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected print('IMU connected') # Register callback imu.set_all_data_period(10) #10ms imu.register_callback(imu.CALLBACK_ALL_DATA, cb_imudynamic) raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.disconnect() # In[ ]: # In[ ]:
class TomIMU: def __init__(self,host,port,uid,callbackPeriodMS=100): self.host=host self.port=port self.uid=uid self._imu = IMU(uid) # Create device object self._ipcon = IPConnection(self.host,self.port) # Create IPconnection to brickd self._ipcon.add_device(self._imu) # Add device to IP connection self.ready = True # Don't use device before it is added to a connection # Set period for quaternion callback (defaults to 100ms) self._imu.set_quaternion_period(callbackPeriodMS) # Register quaternion callback self._imu.register_callback(self._imu.CALLBACK_QUATERNION, self._QuaternionCallback) self._imu.leds_off() # Turn LEDs off. self._imu.set_convergence_speed(5) # 5ms convergence. # Orientation origin and most recent values q = self._imu.get_quaternion() # Get a temp quaternion from current pose. self.rel_x = q.x self.rel_y = q.y self.rel_z = q.z self.rel_w = q.w self.x = q.x self.y = q.y self.z = q.z self.w = q.w def __destroy__(self): self._ipcon.destroy() def _QuaternionCallback(self, x, y, z, w): """ Records the most recent quaternion orientation values. """ self.x,self.y,self.z,self.w = x,y,z,w def SetOrientationOrigin(self, origin=None): """ Resets the orientation origin to the given values, or the latest reading if none. """ if origin is None: self.rel_x, self.rel_y, self.rel_z, self.rel_w = self.x, self.y, self.z, self.w else: self.rel_x, self.rel_y, self.rel_z, self.rel_w = origin def GetEulerOrientation(self): x,y,z,w = self.GetQuaternionOrientation() from math import atan2, asin roll = atan2(2.0*y*w - 2.0*x*z, 1.0 - 2.0*y*y - 2.0*z*z) pitch = atan2(2.0*x*w - 2.0*y*z, 1.0 - 2.0*x*x - 2.0*z*z) yaw = asin(2.0*x*y + 2.0*z*w) return roll,pitch,yaw def GetQuaternionOrientation(self): # Conjugate x,y,z = -self.x, -self.y, -self.z w = self.w # Multiply wn = w * self.rel_w - x * self.rel_x - y * self.rel_y - z * self.rel_z xn = w * self.rel_x + x * self.rel_w + y * self.rel_z - z * self.rel_y yn = w * self.rel_y - x * self.rel_z + y * self.rel_w + z * self.rel_x zn = w * self.rel_z + x * self.rel_y - y * self.rel_x + z * self.rel_w return xn,yn,zn,wn def GetConvergenceSpeed(self): return self._imu.get_convergence_speed() def SetConvergenceSpeed(self, speed): self._imu.set_convergence_speed(speed)
#!/usr/bin/env python # -*- coding: utf-8 -*- HOST = "localhost" PORT = 4223 UID = "ayQskyoNrCW" # Change to your UID from tinkerforge.ip_connection import IPConnection from tinkerforge.brick_imu import IMU # Quaternion callback def cb_quaternion(x, y, z, w): print("x: " + str(x) + "\ny: " + str(y) + "\nz: " + str(z) + "\nw: " + str(w) + "\n") if __name__ == "__main__": ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Set period for quaternion callback to 1s imu.set_quaternion_period(1000) # Register quaternion callback imu.register_callback(imu.CALLBACK_QUATERNION, cb_quaternion) raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.disconnect()
collect(ax, ay, az, rollrate, pitchrate, yawrate, temp) # In[ ]: # In[10]: if __name__ == "__main__": ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected print('IMU connected') # Register callback imu.set_all_data_period(10) #10ms imu.register_callback(imu.CALLBACK_ALL_DATA, cb_imudynamic) raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.disconnect() # In[ ]:
class IMUBarometerFusion(QMainWindow): FACTOR = 1 KP1 = 0.55 * FACTOR # PI observer velocity gain KP2 = 1.0 * FACTOR # PI observer position gain KI = 0.001 / FACTOR # PI observer integral gain (bias cancellation) def __init__(self, parent=None): QMainWindow.__init__(self, parent) self.initialized = False self.altitude_error_i = 0 self.acc_scale = 0.0 self.start_time = time.time() self.altitude_error = 0 self.inst_acceleration = 0.0 self.delta = 0 self.estimated_velocity = 0.0 self.estimated_altitude = 0.0 self.last_time = time.time() self.last_orig_altitude = 0 self.last_estimated_altitude = 0 ipcon = IPConnection() self.imu = IMU(UID_IMU, ipcon) self.barometer = Barometer(UID_BAROMETER, ipcon) ipcon.connect(HOST, PORT) # Turn leds and orientation calculation off, to save calculation time # for the IMU Brick. This makes sure that the measurements are taken # in equidistant 2ms intervals self.imu.leds_off() self.imu.orientation_calculation_off() # Turn averaging of in the Barometer Bricklet to make sure that # the data is without delay self.barometer.set_averaging(0, 0, 0) red_pen = QPen(Qt.red) red_pen.setWidth(5) plot_list = [ #['', Qt.blue, self.get_orig_value], ['', red_pen, self.get_estimated_value] ] self.plot_widget = PlotWidget('Height [m]', plot_list) self.plot_widget.stop = False self.setCentralWidget(self.plot_widget) self.timer = QTimer() self.timer.timeout.connect(self.update) self.timer.start(6) def get_orig_value(self): return self.last_orig_altitude def get_estimated_value(self): return self.last_estimated_altitude # Update measurements and compute new altitude every 6ms. def update(self): q = self.imu.get_quaternion() acc = self.imu.get_acceleration() alt = self.barometer.get_altitude() / 100.0 compensated_acc_q = self.compute_compensated_acc(q, acc) compensated_acc_q_earth = self.compute_dynamic_acceleration_vector( q, compensated_acc_q) self.last_orig_altitude = alt self.last_estimated_altitude = self.compute_altitude( compensated_acc_q_earth[2], alt) # Remove gravity from accelerometer measurements def compute_compensated_acc(self, q, a): g = (2 * (q.x * q.z - q.w * q.y), 2 * (q.w * q.x + q.y * q.z), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) return (a[0] / 1000.0 - g[0], a[1] / 1000.0 - g[1], a[2] / 1000.0 - g[2], 0.0) # Rotate dynamic acceleration vector from sensor frame to earth frame def compute_dynamic_acceleration_vector(self, q, compensated_acc_q): def q_conj(q): return -q[0], -q[1], -q[2], q[3] def q_mult(q1, q2): x1, y1, z1, w1 = q1 x2, y2, z2, w2 = q2 w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2 z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2 return x, y, z, w tmp = q_mult(q, compensated_acc_q) return q_mult(tmp, q_conj(q)) # Computes estimation of altitude based on barometer and accelerometer measurements # Code is based on blog post from Fabio Varesano: http://www.varesano.net/blog/fabio/complementary-filtering-high-res-barometer-and-accelerometer-reliable-altitude-estimation # He seems to have got the idea from the MultiWii project def compute_altitude(self, compensated_acceleration, altitude): self.current_time = time.time() # Initialization if not self.initialized: self.initialized = True self.estimated_altitude = altitude self.estimated_velocity = 0 self.altitude_error_i = 0 # Estimation Error self.altitude_error = altitude - self.estimated_altitude self.altitude_error_i = self.altitude_error_i + self.altitude_error self.altitude_error_i = min(2500.0, max(-2500.0, self.altitude_error_i)) self.inst_acceleration = compensated_acceleration * 9.80665 + self.altitude_error_i * self.KI dt = self.current_time - self.last_time # Integrators self.delta = self.inst_acceleration * dt + (self.KP1 * dt) * self.altitude_error self.estimated_altitude += (self.estimated_velocity / 5.0 + self.delta) * (dt / 2) + ( self.KP2 * dt) * self.altitude_error self.estimated_velocity += self.delta * 10.0 self.last_time = self.current_time return self.estimated_altitude
def collect_data(suffix): global SAMPLE_RATE global w1 global row print "Now recording " + suffix ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Set period for quaternion callback to 1s imu.set_all_data_period(SAMPLE_RATE) imu.set_orientation_period(SAMPLE_RATE) imu.set_quaternion_period(SAMPLE_RATE) f1 = open('data/letters/all_data_'+suffix+'.csv', 'wb') w1 = csv.writer(f1) row = [] # Register quaternion callback imu.register_callback(imu.CALLBACK_ALL_DATA, cb_all_data) imu.register_callback(imu.CALLBACK_ORIENTATION, cb_orientation_data) imu.register_callback(imu.CALLBACK_QUATERNION, cb_quaternion_data) raw_input('Press key to quit recording ' + suffix + ' \n') # Use input() in Python 3 ipcon.disconnect()
class IMUBarometerFusion(QMainWindow): FACTOR = 1 KP1 = 0.55*FACTOR # PI observer velocity gain KP2 = 1.0*FACTOR # PI observer position gain KI = 0.001/FACTOR # PI observer integral gain (bias cancellation) def __init__(self, parent=None): QMainWindow.__init__(self, parent) self.initialized = False self.altitude_error_i = 0 self.acc_scale = 0.0 self.start_time = time.time() self.altitude_error = 0 self.inst_acceleration = 0.0 self.delta = 0 self.estimated_velocity = 0.0 self.estimated_altitude = 0.0 self.last_time = time.time() self.last_orig_altitude = 0 self.last_estimated_altitude = 0 ipcon = IPConnection() self.imu = IMU(UID_IMU, ipcon) self.barometer = Barometer(UID_BAROMETER, ipcon) ipcon.connect(HOST, PORT) # Turn leds and orientation calculation off, to save calculation time # for the IMU Brick. This makes sure that the measurements are taken # in equidistant 2ms intervals self.imu.leds_off() self.imu.orientation_calculation_off() # Turn averaging of in the Barometer Bricklet to make sure that # the data is without delay self.barometer.set_averaging(0, 0, 0) red_pen = QPen(Qt.red) red_pen.setWidth(5) plot_list = [#['', Qt.blue, self.get_orig_value], ['', red_pen, self.get_estimated_value] ] self.plot_widget = PlotWidget('Height [m]', plot_list) self.plot_widget.stop = False self.setCentralWidget(self.plot_widget) self.timer = QTimer() self.timer.timeout.connect(self.update) self.timer.start(6) def get_orig_value(self): return self.last_orig_altitude def get_estimated_value(self): return self.last_estimated_altitude # Update measurements and compute new altitude every 6ms. def update(self): q = self.imu.get_quaternion() acc = self.imu.get_acceleration() alt = self.barometer.get_altitude()/100.0 compensated_acc_q = self.compute_compensated_acc(q, acc) compensated_acc_q_earth = self.compute_dynamic_acceleration_vector(q, compensated_acc_q) self.last_orig_altitude = alt self.last_estimated_altitude = self.compute_altitude(compensated_acc_q_earth[2], alt) # Remove gravity from accelerometer measurements def compute_compensated_acc(self, q, a): g = (2*(q.x*q.z - q.w*q.y), 2*(q.w*q.x + q.y*q.z), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z) return (a[0]/1000.0 - g[0], a[1]/1000.0 - g[1], a[2]/1000.0 - g[2], 0.0) # Rotate dynamic acceleration vector from sensor frame to earth frame def compute_dynamic_acceleration_vector(self, q, compensated_acc_q): def q_conj(q): return -q[0], -q[1], -q[2], q[3] def q_mult(q1, q2): x1, y1, z1, w1 = q1 x2, y2, z2, w2 = q2 w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2 y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2 z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2 return x, y, z, w tmp = q_mult(q, compensated_acc_q) return q_mult(tmp, q_conj(q)) # Computes estimation of altitude based on barometer and accelerometer measurements # Code is based on blog post from Fabio Varesano: http://www.varesano.net/blog/fabio/complementary-filtering-high-res-barometer-and-accelerometer-reliable-altitude-estimation # He seems to have got the idea from the MultiWii project def compute_altitude(self, compensated_acceleration, altitude): self.current_time = time.time() # Initialization if not self.initialized: self.initialized = True self.estimated_altitude = altitude self.estimated_velocity = 0 self.altitude_error_i = 0 # Estimation Error self.altitude_error = altitude - self.estimated_altitude self.altitude_error_i = self.altitude_error_i + self.altitude_error self.altitude_error_i = min(2500.0, max(-2500.0, self.altitude_error_i)) self.inst_acceleration = compensated_acceleration * 9.80665 + self.altitude_error_i * self.KI dt = self.current_time - self.last_time # Integrators self.delta = self.inst_acceleration * dt + (self.KP1 * dt) * self.altitude_error self.estimated_altitude += (self.estimated_velocity/5.0 + self.delta) * (dt / 2) + (self.KP2 * dt) * self.altitude_error self.estimated_velocity += self.delta*10.0 self.last_time = self.current_time return self.estimated_altitude
def calcattitude2(x, y, z, w): # Calculate Attitude # Buchholz, J. J. (2013). Vorlesungsmanuskript Regelungstechnik und Flugregler. # GRIN Verlag. Retrieved from http://www.grin.com/de/e-book/82818/regelungstechnik-und-flugregler yaw = np.arctan2(2.0*(x*y + w*z), w**2+x**2-y**2-z**2) pitch = -np.arcsin(2.0*(w*y - x*z)) roll = -np.arctan2(2.0*(y*z + w*x), -(w**2-x**2-y**2+z**2)) print("\t\t\t\tyaw:\t%+6.0f\troll:\t%+6.0f\tpitch:\t%+6.0f" % (rad2deg(yaw), rad2deg(roll), rad2deg(pitch))) print(80*'=') if __name__ == "__main__": ipcon = IPConnection() # Create IP connection imu = IMU(UID, ipcon) # Create device object ipcon.connect(HOST, PORT) # Connect to brickd # Don't use device before ipcon is connected # Set period for quaternion callback imu.set_quaternion_period(100) imu.set_acceleration_period(100) # Register quaternion callback imu.register_callback(imu.CALLBACK_QUATERNION, calcattitude2) imu.register_callback(imu.CALLBACK_ACCELERATION, calcattitude) raw_input('Press key to exit\n') # Use input() in Python 3 ipcon.disconnect()