collab/pb-hal
git clone https://git.julia.blue/collab/pb-hal
ce16264
imu
1#!/usr/bin/python 2import smbus 3import math 4 5# Register 6power_mgmt_1 = 0x6b 7power_mgmt_2 = 0x6c 8 9def read_byte (reg ): 10return bus .read_byte_data (address ,reg ) 11 12def read_word (reg ): 13h = bus .read_byte_data (address ,reg ) 14l = bus .read_byte_data (address ,reg + 1 ) 15value = (h << 8 )+ l 16return value 17 18def read_word_2c (reg ): 19val = read_word (reg ) 20if (val >= 0x8000 ): 21return - ((65535 - val )+ 1 ) 22else : 23return val 24 25def dist (a ,b ): 26return math .sqrt ((a * a )+ (b * b )) 27 28def get_y_rotation (x ,y ,z ): 29radians = math .atan2 (x ,dist (y ,z )) 30return - math .degrees (radians ) 31 32def get_x_rotation (x ,y ,z ): 33radians = math .atan2 (y ,dist (x ,z )) 34return math .degrees (radians ) 35 36bus = smbus .SMBus (1 )# bus = smbus.SMBus(0) fuer Revision 1 37address = 0x68 # via i2cdetect 38 39# Aktivieren, um das Modul ansprechen zu koennen 40bus .write_byte_data (address ,power_mgmt_1 ,0 ) 41 42"Gyroskop" ) 43"--------" ) 44 45gyroskop_xout = read_word_2c (0x43 ) 46gyroskop_yout = read_word_2c (0x45 ) 47gyroskop_zout = read_word_2c (0x47 ) 48 49"gyroskop_xout: " ,"%5d" % gyroskop_xout ," scaled: " ,gyroskop_xout / 131 ) 50"gyroskop_yout: " ,"%5d" % gyroskop_yout ," scaled: " ,gyroskop_yout / 131 ) 51"gyroskop_zout: " ,"%5d" % gyroskop_zout ," scaled: " ,gyroskop_zout / 131 ) 52 53"Accelerationsensor" ) 55"---------------------" ) 56 57Acceleration_xout = read_word_2c (0x3b ) 58Acceleration_yout = read_word_2c (0x3d ) 59Acceleration_zout = read_word_2c (0x3f ) 60 61Acceleration_xout_scaled = Acceleration_xout / 16384.0 62Acceleration_yout_scaled = Acceleration_yout / 16384.0 63Acceleration_zout_scaled = Acceleration_zout / 16384.0 64 65"Acceleration_xout: " ,"%6d" % Acceleration_xout ," scaled: " ,Acceleration_xout_scaled ) 66"Acceleration_yout: " ,"%6d" % Acceleration_yout ," scaled: " ,Acceleration_yout_scaled ) 67"Acceleration_zout: " ,"%6d" % Acceleration_zout ," scaled: " ,Acceleration_zout_scaled ) 68 69"X Rotation: " ,get_x_rotation (Acceleration_xout_scaled ,Acceleration_yout_scaled ,Acceleration_zout_scaled )) 70"Y Rotation: " ,get_y_rotation (Acceleration_xout_scaled ,Acceleration_yout_scaled ,Acceleration_zout_scaled )) 71 72""" 73Sample Data Output Expected 74Gyroskop 75-------- 76gyroskop_xout: -260 scaled: -2 77gyroskop_yout: -154 scaled: -2 78gyroskop_zout: 78 scaled: 0 79 80Accelerationsensor 81--------------------- 82Acceleration_xout: -1048 scaled: -0.06396484375 83Acceleration_yout: -676 scaled: -0.041259765625 84Acceleration_zout: 16644 scaled: 1.01586914062 85X Rotation: -2.32121150537 86Y Rotation: 3.59994842011 87"""