From d74412d774ed3855d73e4aa7bac30066513499ad Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 5 Mar 2015 18:13:21 +0000 Subject: [PATCH 01/22] MPU9250 library up and running --- bbio/libraries/MPU9250/MPU9250.py | 159 +++++++++++++++++++++++++++++ bbio/libraries/MPU9250/__init__.py | 2 + examples/MPU9250_test.py | 50 +++++++++ 3 files changed, 211 insertions(+) create mode 100755 bbio/libraries/MPU9250/MPU9250.py create mode 100755 bbio/libraries/MPU9250/__init__.py create mode 100644 examples/MPU9250_test.py diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py new file mode 100755 index 0000000..0a2ff1c --- /dev/null +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -0,0 +1,159 @@ +""" +MPU9250 +Copyright 2015 - Niko Visnjic + +A PyBBIO library for controlling the MPU 9250 9-DOF sensor via SPI. + +MPU9250 is released as part of PyBBIO under its MIT license. +See PyBBIO/LICENSE.txt +""" +import bbio + +class MPU9250(object): + ID_VALUE = 0x71 # precoded identification string in WHOAMI Reg + + REG_ID = 0x75 # WHOAMI + + REG_I2C_MST_CTRL = 0x24 + REG_I2C_SLV0_ADDR = 0x25 + REG_I2C_SLV0_REG = 0x26 + REG_I2C_SLV0_CTRL = 0x27 + REG_I2C_SLV0_DO = 0x63 + REG_USER_CTRL = 0x6A + + AK8963_CNTL1 = 0x0A + AK8963_CNTL2 = 0x0B + + + CMD_TEMPERATURE = 0x2e + CMD_PRESSURE = 0x34 + CMD_PRESSURE_OSS_INC = 0x40 + + TEMP_CONVERSION_TIME = 4.5 + PRESSURE_CONVERSION_TIMES = [4.5, 7.5, 13.5, 25.5] + + OVERSAMPLE_0 = 0 + OVERSAMPLE_1 = 1 + OVERSAMPLE_2 = 2 + OVERSAMPLE_3 = 3 + + RANGE_ACCEL = 16 # Gs + RANGE_GYRO = 2000 # dps + RANGE_MAG = 4912 # uT + + def __init__(self, spi, cs=0): + self.spi = spi + self.cs = cs + spi.begin() + spi.setClockMode(0, 0) + spi.setMaxFrequency(0, 3000000) + id_val = self.readRegister(self.REG_ID)[0] + # print "\nGot WHOAMI = 0x%02x" %id_val + assert id_val == self.ID_VALUE, "MPU9250 not detected on SPI bus" + + #This part is to set up the magnetometer, due to it being a separate device + self.writeRegister(self.REG_I2C_MST_CTRL, 0x0D) # I2C speed 400 Khz + self.writeRegister(self.REG_USER_CTRL, 0x22) # + + # Reset magnetometer + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL2) + self.writeRegister(self.REG_I2C_SLV0_DO, 0x01) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + # Set 16-bit continues MODE1 readouts + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + + + def ak8963Whoami( self): + """ I2C WhoAmI check for the AK8963 in-built magnetometer """ + + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + # READ Flag + 0x0C is AK slave addr + self.writeRegister(self.REG_I2C_SLV0_REG, 0x00) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + whoami_ak = self.readRegister(73, 1) + print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] + assert whoami_ak[0] == 0x48, "AK8963 not detected on internal I2C bus" + + def getAcceleration( self): + """ Returns current acceleration triplet. """ + msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) + + valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_ACCEL + valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_ACCEL + valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_ACCEL + return [valX, valY, valZ] + + def getGyro( self): + """ Returns current acceleration triplet. """ + msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(67, 6) + + valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_GYRO + valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_GYRO + valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_GYRO + return [valX, valY, valZ] + + def getMag( self): + """ Returns current acceleration triplet. """ + # bit more fun since we're reading via internal I2C + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + self.writeRegister(self.REG_I2C_SLV0_REG, 0x03) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x87) # read 7 + msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) + + valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_MAG + valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_MAG + valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_MAG + return [valX, valY, valZ] + + + def getTemp(self, return_b5_coefficient=False): + """ Returns current temperature in Celsius. """ + self.writeRegister(self.REG_CTRL_MEAS, self.CMD_TEMPERATURE) + bbio.delay(self.TEMP_CONVERSION_TIME) + msb, lsb = self.readRegister(self.REG_OUT_MSB, 2) + val = (msb << 8) | lsb + + # Conversions are straight out of the datasheet + x1 = (val - self.cal_AC6) * self.cal_AC5 / 32768 + x2 = self.cal_MC * 2048 / (x1 + self.cal_MD) + b5 = x1 + x2 + temp_counts = (b5 + 8) / 16 + temp = temp_counts * 0.1 # 0.1 degree C resolution + if return_b5_coefficient: + return (temp, b5) + return temp + + def getTempF(self): + """ Returns current temperature in Fahrenheit. """ + tempC = self.readTemp() + return tempC * 9./5 + 32 + + + + def readRegister(self, addr, n_bytes=1): + """ Reads the value in the given register, or if the optional parameter + 'n_bytes' is greater than 1 returns n_bytes register values starting + at given address. """ + assert n_bytes > 0, "Can't read less than 1 byte!" + request = [addr | 0x80] + [0]*n_bytes + response = self.spi.transfer(self.cs, request) + return response[1:] # slice off value read when writing register + + def writeRegister(self, addr, value): + """ Writes the given value to the given register. """ + self.spi.write(self.cs, [addr & 0x7f, value & 0xff]) + + +def fromUnsigned16(bytes): + """ Convert register values as unsigned short int """ + return bytes[0]<<8 | bytes[1] + +def fromSigned16(bytes): + """ Convert register values as signed short int """ + x = fromUnsigned16(bytes) + if x > 32767: return -(65536-x) + return x + diff --git a/bbio/libraries/MPU9250/__init__.py b/bbio/libraries/MPU9250/__init__.py new file mode 100755 index 0000000..1036368 --- /dev/null +++ b/bbio/libraries/MPU9250/__init__.py @@ -0,0 +1,2 @@ +# __init__.py file for PyBBIO's MPU9250 library +from MPU9250 import MPU9250 diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py new file mode 100644 index 0000000..1241536 --- /dev/null +++ b/examples/MPU9250_test.py @@ -0,0 +1,50 @@ +""" +MPU9250_test.py +Niko Visnjic + +Short demonstration on how to use the MPU9250 library included +with PyBBIO. Interfaces with the MPU9250 9-DOF sensor to measure +accelerometer, gyroscope and magnetometer data. + +This ecample program is in the public domain. +""" + +from bbio import * +# Import the MPU9250 class from the MPU9250 library: +from bbio.libraries.MPU9250 import MPU9250 + +# Create a new instance of the MPU9250 class using SPI0 with the +# default to CS0 chip select pin: +mpu = MPU9250(SPI0) + +def setup(): + + # Setup accel and gyro for full range + mpu.writeRegister( 27, 24) # GryoConfig = 0b00011000 + mpu.writeRegister( 28, 24) # AccelConfig1 = 0b00011000 + + confData = mpu.readRegister( 27, 2) + print '\n GyroConfig: {:#010b}'.format(confData[0]) + print '\n AccelConfig: {:#010b}'.format(confData[1]) + + delay(100) # Let the I2C reset settle + # Sanity check + mpu.ak8963Whoami() + +def loop(): + + # Get data + accelX, accelY, accelZ = mpu.getAcceleration() + gyroX, gyroY, gyroZ = mpu.getGyro() + magX, magY, magZ = mpu.getMag() + + # Test print for conveinence + + print "\n===============================================================================\n" + print "\n AccelX: %02f Gs \t | AccelY: %02f Gs\t | AccelZ: %02f Gs" % (accelX, accelY, accelZ ) + print "\n GyroX: %02f dps \t | GyroY: %02f dps\t | GyroZ: %02f dps" % (gyroX, gyroY, gyroZ ) + print "\n MagX: %02f uT \t | MagY: %02f uT\t | MagZ: %02f uT" % (magX, magY, magZ ) + + delay(200) + +run(setup, loop) From 0617fe620e30c76419bafc65744b0f1c1281f67d Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Wed, 29 Jul 2015 18:00:54 +0800 Subject: [PATCH 02/22] fixed float formatting and time stamp --- examples/MPU9250_test.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 1241536..313aa7e 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -41,9 +41,9 @@ def loop(): # Test print for conveinence print "\n===============================================================================\n" - print "\n AccelX: %02f Gs \t | AccelY: %02f Gs\t | AccelZ: %02f Gs" % (accelX, accelY, accelZ ) - print "\n GyroX: %02f dps \t | GyroY: %02f dps\t | GyroZ: %02f dps" % (gyroX, gyroY, gyroZ ) - print "\n MagX: %02f uT \t | MagY: %02f uT\t | MagZ: %02f uT" % (magX, magY, magZ ) + print "\n AccelX: %.3f Gs \t | AccelY: %.3f Gs\t | AccelZ: %.3f Gs" % (accelX, accelY, accelZ ) + print "\n GyroX: %.3f dps \t | GyroY: %.3f dps\t | GyroZ: %.3f dps" % (gyroX, gyroY, gyroZ ) + print "\n MagX: %.3f uT \t | MagY: %.3f uT\t | MagZ: %.3f uT" % (magX, magY, magZ ) delay(200) From d8308aa84b2649ee1a80c39853c8208d806ec7db Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 30 Jul 2015 17:13:29 +0800 Subject: [PATCH 03/22] Added getTemp() to library; moved fromSigned16() to method --- bbio/libraries/MPU9250/MPU9250.py | 61 +++++++++++++++---------------- examples/MPU9250_test.py | 4 ++ 2 files changed, 33 insertions(+), 32 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 0a2ff1c..d6623e7 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -24,6 +24,8 @@ class MPU9250(object): AK8963_CNTL1 = 0x0A AK8963_CNTL2 = 0x0B + REG_TEMP_OUT_H = 0x41 + REG_TEMP_OUT_L = 0x42 CMD_TEMPERATURE = 0x2e CMD_PRESSURE = 0x34 @@ -81,18 +83,18 @@ def getAcceleration( self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) - valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_ACCEL - valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_ACCEL - valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_ACCEL + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_ACCEL + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_ACCEL + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_ACCEL return [valX, valY, valZ] def getGyro( self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(67, 6) - valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_GYRO - valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_GYRO - valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_GYRO + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_GYRO + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_GYRO + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_GYRO return [valX, valY, valZ] def getMag( self): @@ -103,27 +105,22 @@ def getMag( self): self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x87) # read 7 msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) - valX = fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_MAG - valY = fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_MAG - valZ = fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_MAG + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_MAG + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_MAG + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_MAG return [valX, valY, valZ] - def getTemp(self, return_b5_coefficient=False): - """ Returns current temperature in Celsius. """ - self.writeRegister(self.REG_CTRL_MEAS, self.CMD_TEMPERATURE) - bbio.delay(self.TEMP_CONVERSION_TIME) - msb, lsb = self.readRegister(self.REG_OUT_MSB, 2) - val = (msb << 8) | lsb - - # Conversions are straight out of the datasheet - x1 = (val - self.cal_AC6) * self.cal_AC5 / 32768 - x2 = self.cal_MC * 2048 / (x1 + self.cal_MD) - b5 = x1 + x2 - temp_counts = (b5 + 8) / 16 - temp = temp_counts * 0.1 # 0.1 degree C resolution - if return_b5_coefficient: - return (temp, b5) + def getTemp(self): + """ Returns current temperature of sensor die in Celsius. """ + msb, lsb = self.readRegister(self.REG_TEMP_OUT_H, 2) + val = self.fromSigned16([msb, lsb]) + + # Conversions are straight out of the datahsheet + # TEMP_degC = ((TEMP_OUT - RoomTemp_Offset) / Temp_Sensitivity) + 21 + # They fail to mention what RT_Offset or Temp_Sensitivity should equal to + temp = (( val - 21) / 333.87) + 21; + return temp def getTempF(self): @@ -147,13 +144,13 @@ def writeRegister(self, addr, value): self.spi.write(self.cs, [addr & 0x7f, value & 0xff]) -def fromUnsigned16(bytes): - """ Convert register values as unsigned short int """ - return bytes[0]<<8 | bytes[1] + def fromUnsigned16(self, bytes): + """ Convert register values as unsigned short int """ + return bytes[0]<<8 | bytes[1] -def fromSigned16(bytes): - """ Convert register values as signed short int """ - x = fromUnsigned16(bytes) - if x > 32767: return -(65536-x) - return x + def fromSigned16(self, bytes): + """ Convert register values as signed short int """ + x = self.fromUnsigned16(bytes) + if x > 32767: return -(65536-x) + return x diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 313aa7e..f6946d5 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -45,6 +45,10 @@ def loop(): print "\n GyroX: %.3f dps \t | GyroY: %.3f dps\t | GyroZ: %.3f dps" % (gyroX, gyroY, gyroZ ) print "\n MagX: %.3f uT \t | MagY: %.3f uT\t | MagZ: %.3f uT" % (magX, magY, magZ ) + degC = mpu.getTemp() + + print "\n On-die temperature is: %d C" % degC + delay(200) run(setup, loop) From 4f6323572520bd2081a7b6534c1b0f18017f8611 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 30 Jul 2015 23:56:17 +0800 Subject: [PATCH 04/22] added setRange() methods, still have to fix the backward enum convesion --- bbio/libraries/MPU9250/MPU9250.py | 83 ++++++++++++++++++++++--------- examples/MPU9250_test.py | 13 ++++- 2 files changed, 71 insertions(+), 25 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index d6623e7..17b5690 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -27,6 +27,10 @@ class MPU9250(object): REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 + REG_GYRO_CONFIG = 0x1B + REG_ACCEL_CONFIG1 = 0x1C + REG_ACCEL_CONFIG2 = 0x1D + CMD_TEMPERATURE = 0x2e CMD_PRESSURE = 0x34 CMD_PRESSURE_OSS_INC = 0x40 @@ -34,21 +38,14 @@ class MPU9250(object): TEMP_CONVERSION_TIME = 4.5 PRESSURE_CONVERSION_TIMES = [4.5, 7.5, 13.5, 25.5] - OVERSAMPLE_0 = 0 - OVERSAMPLE_1 = 1 - OVERSAMPLE_2 = 2 - OVERSAMPLE_3 = 3 - - RANGE_ACCEL = 16 # Gs - RANGE_GYRO = 2000 # dps - RANGE_MAG = 4912 # uT - def __init__(self, spi, cs=0): self.spi = spi self.cs = cs spi.begin() spi.setClockMode(0, 0) spi.setMaxFrequency(0, 3000000) + + # Am I talking to an MPU9250? id_val = self.readRegister(self.REG_ID)[0] # print "\nGot WHOAMI = 0x%02x" %id_val assert id_val == self.ID_VALUE, "MPU9250 not detected on SPI bus" @@ -66,9 +63,28 @@ def __init__(self, spi, cs=0): self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + + # Define possible gyro & accel ranges + self.RANGE_GYRO = self.enum( DPS2000=3, DPS1000=2, DPS500=1, DPS250=0) + self.RANGE_ACCEL = self.enum( G16=3, G8=2, G4=1, G2=0) + + # RANGE_ACCEL = 16 # Gs + # RANGE_GYRO = 2000 # dps + + # Set default range to max + #RANGE_CURR_GYRO = 0 + #RANGE_CURR_ACCEL = 0 + # fix this? + self.RANGE_CURR_GYRO = self.RANGE_GYRO.DPS2000 + self.RANGE_CURR_ACCEL = self.RANGE_ACCEL.G16 + self.RANGE_CURR_MAG = 4912 # uT + + + self.setRangeGyro(self.RANGE_CURR_GYRO) + self.setRangeAccel(self.RANGE_CURR_ACCEL) + - - def ak8963Whoami( self): + def ak8963Whoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) @@ -79,22 +95,38 @@ def ak8963Whoami( self): print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] assert whoami_ak[0] == 0x48, "AK8963 not detected on internal I2C bus" - def getAcceleration( self): + def setRangeGyro(self, rangeVal): + """ Sets the readout range for the gyroscope + Possible values, rangeVal == RANGE_GYRO\.2000DPS\.1000DPS\.500DPS\.250DPS + """ + self.writeRegister(self.REG_GYRO_CONFIG, rangeVal) + + gyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1) + # test printout + print "\n I've set the gyro range to %d, wanted to set to %d" %(gyroConf[0], rangeVal) + + def setRangeAccel(self, rangeVal): + """ Sets the readout range for the accelerometer + Possible values, rangeVal == RANGE_ACCEL\.16Gs\.8Gs\.4Gs\.2Gs + """ + self.writeRegister( self.REG_ACCEL_CONFIG1, rangeVal) + + def getAcceleration(self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_ACCEL - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_ACCEL - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_ACCEL + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_ACCEL + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_ACCEL + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_ACCEL return [valX, valY, valZ] - def getGyro( self): + def getGyro(self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(67, 6) - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_GYRO - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_GYRO - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_GYRO + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_GYRO + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_GYRO + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_GYRO return [valX, valY, valZ] def getMag( self): @@ -105,12 +137,11 @@ def getMag( self): self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x87) # read 7 msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_MAG - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_MAG - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_MAG + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_MAG + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_MAG + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_MAG return [valX, valY, valZ] - def getTemp(self): """ Returns current temperature of sensor die in Celsius. """ msb, lsb = self.readRegister(self.REG_TEMP_OUT_H, 2) @@ -128,7 +159,10 @@ def getTempF(self): tempC = self.readTemp() return tempC * 9./5 + 32 + def runSelfTestGyro(self): + """ Initiates self test for Gyroscope and returns self-test values """ + def readRegister(self, addr, n_bytes=1): """ Reads the value in the given register, or if the optional parameter @@ -154,3 +188,6 @@ def fromSigned16(self, bytes): if x > 32767: return -(65536-x) return x + def enum(self, **enums): + """ Using enums for an API-like library is (usually) a good idea, hence """ + return type('Enum', (), enums) diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index f6946d5..748ee65 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -20,17 +20,26 @@ def setup(): # Setup accel and gyro for full range - mpu.writeRegister( 27, 24) # GryoConfig = 0b00011000 - mpu.writeRegister( 28, 24) # AccelConfig1 = 0b00011000 + # mpu.writeRegister( 27, 24) # GryoConfig = 0b00011000 + # mpu.writeRegister( 28, 24) # AccelConfig1 = 0b00011000 + # Change gyro range for fun + mpu.setRangeGyro(mpu.RANGE_GYRO.DPS500) + + delay(1000) confData = mpu.readRegister( 27, 2) print '\n GyroConfig: {:#010b}'.format(confData[0]) print '\n AccelConfig: {:#010b}'.format(confData[1]) + # Change it back to maximum + mpu.setRangeGyro(mpu.RANGE_GYRO.DPS2000) + delay(100) # Let the I2C reset settle # Sanity check mpu.ak8963Whoami() + + def loop(): # Get data From 0ec43d5139eee7e47808ef27efad4810ab237930 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 31 Jul 2015 05:02:26 +0800 Subject: [PATCH 05/22] added setRange() and cleaned up a bit --- bbio/libraries/MPU9250/MPU9250.py | 120 ++++++++++++++++++------------ examples/MPU9250_test.py | 13 +++- 2 files changed, 82 insertions(+), 51 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 17b5690..21f4f9d 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -31,19 +31,27 @@ class MPU9250(object): REG_ACCEL_CONFIG1 = 0x1C REG_ACCEL_CONFIG2 = 0x1D - CMD_TEMPERATURE = 0x2e - CMD_PRESSURE = 0x34 - CMD_PRESSURE_OSS_INC = 0x40 - TEMP_CONVERSION_TIME = 4.5 - PRESSURE_CONVERSION_TIMES = [4.5, 7.5, 13.5, 25.5] - + # Define possible gyro & accel ranges + RANGE_GYRO_2000DPS = 3 + RANGE_GYRO_1000DPS = 2 + RANGE_GYRO_500DPS = 1 + RANGE_GYRO_250DPS = 0 + RANGE_ACCEL_16G = 3 + RANGE_ACCEL_8G = 2 + RANGE_ACCEL_4G = 1 + RANGE_ACCEL_2G = 0 + + # Same as above, used for scaling + SCALE_GYRO = 250, 500, 1000, 2000 + SCALE_ACCEL = 2, 4, 8, 16 + def __init__(self, spi, cs=0): self.spi = spi self.cs = cs spi.begin() spi.setClockMode(0, 0) - spi.setMaxFrequency(0, 3000000) + spi.setMaxFrequency(0, 1000000) # Am I talking to an MPU9250? id_val = self.readRegister(self.REG_ID)[0] @@ -64,29 +72,21 @@ def __init__(self, spi, cs=0): self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - # Define possible gyro & accel ranges - self.RANGE_GYRO = self.enum( DPS2000=3, DPS1000=2, DPS500=1, DPS250=0) - self.RANGE_ACCEL = self.enum( G16=3, G8=2, G4=1, G2=0) - # RANGE_ACCEL = 16 # Gs - # RANGE_GYRO = 2000 # dps - - # Set default range to max - #RANGE_CURR_GYRO = 0 - #RANGE_CURR_ACCEL = 0 - # fix this? - self.RANGE_CURR_GYRO = self.RANGE_GYRO.DPS2000 - self.RANGE_CURR_ACCEL = self.RANGE_ACCEL.G16 - self.RANGE_CURR_MAG = 4912 # uT - - - self.setRangeGyro(self.RANGE_CURR_GYRO) - self.setRangeAccel(self.RANGE_CURR_ACCEL) + # Set current ranges to max values + + self.currentRangeMag = 4912 # uT; fixed! + self.setRangeGyro(self.RANGE_GYRO_2000DPS) + self.setRangeAccel(self.RANGE_ACCEL_16G) + + # Above code does these + #self.CurrentRangeGyro = self.RANGE_GYRO_2000DPS + #self.CurrentRangeAccel = self.RANGE_ACCEL_16G + # Done with init() def ak8963Whoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) # READ Flag + 0x0C is AK slave addr self.writeRegister(self.REG_I2C_SLV0_REG, 0x00) @@ -97,36 +97,63 @@ def ak8963Whoami(self): def setRangeGyro(self, rangeVal): """ Sets the readout range for the gyroscope - Possible values, rangeVal == RANGE_GYRO\.2000DPS\.1000DPS\.500DPS\.250DPS + Possible values + rangeVal == self.RANGE_GYRO_\2000DPS\1000DPS\500DPS\250DPS """ - self.writeRegister(self.REG_GYRO_CONFIG, rangeVal) - - gyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1) - # test printout - print "\n I've set the gyro range to %d, wanted to set to %d" %(gyroConf[0], rangeVal) + regVal = rangeVal<<3 # Shift to bits [3:4] GYRO_FS_SEL + self.writeRegister(self.REG_GYRO_CONFIG, regVal) + + gyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] + # test if we did it right?? + if (regVal == gyroConf): + # Update Current range variable + self.currentRangeGyro = rangeVal + else: + print "\n WARNING! FAILED TO SET gyroscope range!" + print "\n\tI've set REG_GYRO_CONFIG to %d, wanted to set to %d" % ( + gyroConf, regVal) + + # @TODO Add proper error log def setRangeAccel(self, rangeVal): - """ Sets the readout range for the accelerometer - Possible values, rangeVal == RANGE_ACCEL\.16Gs\.8Gs\.4Gs\.2Gs + """ Sets the readout range for the accelerometer + Possible values + rangeVal == self.RANGE_GYRO_\16G\8G\4G\2G """ - self.writeRegister( self.REG_ACCEL_CONFIG1, rangeVal) + regVal = rangeVal<<3 # Shift to bits [3:4] ACCEL_FS_SEL + self.writeRegister(self.REG_ACCEL_CONFIG1, regVal) + + accelConf = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] + # test if we did it right?? + if (regVal == accelConf): + # Update Current range variable + self.currentRangeAccel = rangeVal + else: + print "\n WARNING! FAILED TO SET acceletometer range!" + print "\n\tI've set REG_ACCEL_CONFIG to %d, wanted to set to %d" % ( + accelConf, regVal) + + # @TODO Add proper error log + def getAcceleration(self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_ACCEL - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_ACCEL - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_ACCEL + scaling = self.SCALE_ACCEL[self.currentRangeAccel] # Get scale value + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling return [valX, valY, valZ] def getGyro(self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(67, 6) - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_GYRO - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_GYRO - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_GYRO + scaling = self.SCALE_GYRO[self.currentRangeGyro] # Get scale value + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling return [valX, valY, valZ] def getMag( self): @@ -136,10 +163,11 @@ def getMag( self): self.writeRegister(self.REG_I2C_SLV0_REG, 0x03) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x87) # read 7 msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) - - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * self.RANGE_CURR_MAG - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * self.RANGE_CURR_MAG - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * self.RANGE_CURR_MAG + + scaling = self.currentRangeMag + valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling return [valX, valY, valZ] def getTemp(self): @@ -162,7 +190,6 @@ def getTempF(self): def runSelfTestGyro(self): """ Initiates self test for Gyroscope and returns self-test values """ - def readRegister(self, addr, n_bytes=1): """ Reads the value in the given register, or if the optional parameter @@ -188,6 +215,3 @@ def fromSigned16(self, bytes): if x > 32767: return -(65536-x) return x - def enum(self, **enums): - """ Using enums for an API-like library is (usually) a good idea, hence """ - return type('Enum', (), enums) diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 748ee65..9709041 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -24,7 +24,7 @@ def setup(): # mpu.writeRegister( 28, 24) # AccelConfig1 = 0b00011000 # Change gyro range for fun - mpu.setRangeGyro(mpu.RANGE_GYRO.DPS500) + mpu.setRangeGyro(mpu.RANGE_GYRO_500DPS) delay(1000) confData = mpu.readRegister( 27, 2) @@ -32,7 +32,14 @@ def setup(): print '\n AccelConfig: {:#010b}'.format(confData[1]) # Change it back to maximum - mpu.setRangeGyro(mpu.RANGE_GYRO.DPS2000) + mpu.setRangeGyro(mpu.RANGE_GYRO_2000DPS) + + delay(1000) + confData = mpu.readRegister( 27, 2) + print '\n GyroConfig: {:#010b}'.format(confData[0]) + print '\n AccelConfig: {:#010b}'.format(confData[1]) + + delay(1000) delay(100) # Let the I2C reset settle # Sanity check @@ -41,7 +48,7 @@ def setup(): def loop(): - + # Get data accelX, accelY, accelZ = mpu.getAcceleration() gyroX, gyroY, gyroZ = mpu.getGyro() From 4c258744f9d2d8622974d721292c146412000d63 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 31 Jul 2015 13:30:48 +0800 Subject: [PATCH 06/22] fixed wrong divisor value --- bbio/libraries/MPU9250/MPU9250.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 21f4f9d..d85dbb8 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -141,9 +141,9 @@ def getAcceleration(self): msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) scaling = self.SCALE_ACCEL[self.currentRangeAccel] # Get scale value - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling + valX = self.fromSigned16([msbX, lsbX]) / 32768.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32768.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32768.0 * scaling return [valX, valY, valZ] def getGyro(self): @@ -151,9 +151,9 @@ def getGyro(self): msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(67, 6) scaling = self.SCALE_GYRO[self.currentRangeGyro] # Get scale value - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling + valX = self.fromSigned16([msbX, lsbX]) / 32768.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32768.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32768.0 * scaling return [valX, valY, valZ] def getMag( self): @@ -165,9 +165,9 @@ def getMag( self): msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) scaling = self.currentRangeMag - valX = self.fromSigned16([msbX, lsbX]) / 32760.0 * scaling - valY = self.fromSigned16([msbY, lsbY]) / 32760.0 * scaling - valZ = self.fromSigned16([msbZ, lsbZ]) / 32760.0 * scaling + valX = self.fromSigned16([msbX, lsbX]) / 32768.0 * scaling + valY = self.fromSigned16([msbY, lsbY]) / 32768.0 * scaling + valZ = self.fromSigned16([msbZ, lsbZ]) / 32768.0 * scaling return [valX, valY, valZ] def getTemp(self): From 078d6cdfe10160fc4016e43e6d86ce72d2b9a2cf Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 31 Jul 2015 15:28:35 +0800 Subject: [PATCH 07/22] Added runSelfTest() method; all sensors within spec --- bbio/libraries/MPU9250/MPU9250.py | 124 ++++++++++++++++++++++++++++-- examples/MPU9250_test.py | 30 +++++--- 2 files changed, 135 insertions(+), 19 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index d85dbb8..d028dd9 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -14,6 +14,13 @@ class MPU9250(object): REG_ID = 0x75 # WHOAMI + REG_SMPLRT_DIV = 0x19 + REG_CONFIG = 0x18 + REG_GYRO_CONFIG = 0x1B + REG_ACCEL_CONFIG1 = 0x1C + REG_ACCEL_CONFIG2 = 0x1D + + REG_FIFO_EN = 0x23 REG_I2C_MST_CTRL = 0x24 REG_I2C_SLV0_ADDR = 0x25 REG_I2C_SLV0_REG = 0x26 @@ -27,9 +34,6 @@ class MPU9250(object): REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 - REG_GYRO_CONFIG = 0x1B - REG_ACCEL_CONFIG1 = 0x1C - REG_ACCEL_CONFIG2 = 0x1D # Define possible gyro & accel ranges @@ -136,7 +140,7 @@ def setRangeAccel(self, rangeVal): # @TODO Add proper error log - def getAcceleration(self): + def getAccel(self): """ Returns current acceleration triplet. """ msbX, lsbX, msbY, lsbY, msbZ, lsbZ = self.readRegister(59, 6) @@ -187,9 +191,115 @@ def getTempF(self): tempC = self.readTemp() return tempC * 9./5 + 32 - def runSelfTestGyro(self): - """ Initiates self test for Gyroscope and returns self-test values """ - + def runSelfTest(self): + """ Initiates self test for gyroscope and accelerometer + Returns deviation with respect to factory trim values + """ + + # Init sums + sumAccel = [0] * 200 + sumGyro = [0] * 200 + STSumAccel = [0] * 200 + STSumGyro = [0] * 200 + + # Set range and sampling rate + # Set gyro sample rate to 1 kHz + self.writeRegister(self.REG_SMPLRT_DIV, 0x00) + # Set gyro sample rate to 1 kHz and DLPF to 92 Hz + self.writeRegister(self.REG_CONFIG, 0x02) + # Set full scale range for the gyro to 250 dps + self.setRangeGyro(self.RANGE_GYRO_250DPS) + # Set accelerometer rate to 1 kHz and bandwidth to 92 Hz + self.writeRegister(self.REG_ACCEL_CONFIG2, 0x02) + # Set full scale range for the accelerometer to 2 Gs + self.setRangeAccel(self.RANGE_ACCEL_2G) + + + # Get 200 measurements for averaging + for i in range(200): + + accelX, accelY, accelZ = self.getAccel() + gyroX, gyroY, gyroZ = self.getGyro() + + sumAccel[0] += accelX + sumAccel[1] += accelY + sumAccel[2] += accelZ + sumGyro[0] += gyroX + sumGyro[1] += gyroY + sumGyro[2] += gyroZ + + for i in range(3): + sumAccel[i] /= 200.0 + sumGyro[i] /= 200.0 + + + # print " AVG Accel = %f %f %f | AVG Gyro = %f %f %f" % ( sumAccel[0],sumAccel[1],sumAccel[2], sumGyro[0], sumGyro[1],sumGyro[2]) + + + # Configure accel & gyro for self-test + # Enable self test on all three axes and set accel range to +/- 2 g + self.writeRegister(self.REG_ACCEL_CONFIG1, 0xE0) + # Enable self test on all three axes and set gyro range to +/- 250 DPS + self.writeRegister(self.REG_GYRO_CONFIG, 0xE0) + + # Get 200 self-test measurements for averaging + for i in range(200): + + accelX, accelY, accelZ = self.getAccel() + gyroX, gyroY, gyroZ = self.getGyro() + + STSumAccel[0] += accelX + STSumAccel[1] += accelY + STSumAccel[2] += accelZ + STSumGyro[0] += gyroX + STSumGyro[1] += gyroY + STSumGyro[2] += gyroZ + + for i in range(3): + STSumAccel[i] /= 200.0 + STSumGyro[i] /= 200.0 + + + # Set things back to normal again + self.setRangeGyro(self.RANGE_GYRO_250DPS) # Write 0x00 to gyro config + self.setRangeAccel(self.RANGE_ACCEL_2G) # Write 0x00 to accel config + + # Retrieve factory self-test data + STGyroX, STGyroY, STGyroZ = self.readRegister(0x00, 3) + STAccelX, STAccelY, STAccelZ = self.readRegister(0x0D, 3) + + FTrimGyro = [0] * 3 + FTrimAccel = [0] * 3 + # Compute factory trim values + FTrimGyro[0] = 2620 * pow( 1.01, STGyroX - 1.0 ) + FTrimGyro[1] = 2620 * pow( 1.01, STGyroY - 1.0 ) + FTrimGyro[2] = 2620 * pow( 1.01, STGyroZ - 1.0 ) + FTrimAccel[0] = 2620 * pow( 1.01, STAccelX - 1.0 ) + FTrimAccel[1] = 2620 * pow( 1.01, STAccelY - 1.0 ) + FTrimAccel[2] = 2620 * pow( 1.01, STAccelZ - 1.0 ) + + devGyro = [0] * 3 + devAccel = [0] * 3 + # Compute deviations + for i in range(3): + devAccel[i] = 100.0 * ( STSumAccel[i] - sumAccel[i]) / FTrimAccel[i] + devGyro[i] = 100.0 * ( STSumGyro[i] - sumGyro[i]) / FTrimGyro[i] + + return devAccel, devGyro + + """ + // Configure the gyro and accelerometer for normal operation + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); + delay(25); // Delay a while to let the device stabilize + """ + """ + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< Date: Sat, 1 Aug 2015 09:55:24 +0800 Subject: [PATCH 08/22] added range check to setRange() and register preservation to runSelfTest() --- bbio/libraries/MPU9250/MPU9250.py | 124 +++++++++++++++++++----------- 1 file changed, 78 insertions(+), 46 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index d028dd9..a6a7e23 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -76,7 +76,13 @@ def __init__(self, spi, cs=0): self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - + + # Reset gyro and accel configs + self.writeRegister(self.REG_GYRO_CONFIG, 0x00) + self.writeRegister(self.REG_ACCEL_CONFIG1, 0x00) + self.writeRegister(self.REG_ACCEL_CONFIG2, 0x00) + + # Set current ranges to max values self.currentRangeMag = 4912 # uT; fixed! @@ -104,40 +110,64 @@ def setRangeGyro(self, rangeVal): Possible values rangeVal == self.RANGE_GYRO_\2000DPS\1000DPS\500DPS\250DPS """ - regVal = rangeVal<<3 # Shift to bits [3:4] GYRO_FS_SEL - self.writeRegister(self.REG_GYRO_CONFIG, regVal) - - gyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] - # test if we did it right?? - if (regVal == gyroConf): - # Update Current range variable - self.currentRangeGyro = rangeVal + # We only accept rangeVal = 0 to 3 + if (rangeVal < 0 or rangeVal > 3): + print "\n Invalid RANGE value! Range not changed" + # @TODO Error handling + return -1 else: - print "\n WARNING! FAILED TO SET gyroscope range!" - print "\n\tI've set REG_GYRO_CONFIG to %d, wanted to set to %d" % ( - gyroConf, regVal) - - # @TODO Add proper error log + + # Preserve previous REG bits + regOld = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] + regOld &= ~(0x3<<3) # Clear [3:4] FS bits + # Combine regOld and rangeVal shifted to bits [3:4] GYRO_FS_SEL + regVal = regOld | (rangeVal<<3) + self.writeRegister(self.REG_GYRO_CONFIG, regVal) + + gyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] + # test if we did it right?? + if (regVal == gyroConf): + # Update Current range variable + self.currentRangeGyro = rangeVal + return 0 + else: + print "\n WARNING! FAILED TO SET gyroscope range!" + print "\n\tI've set REG_GYRO_CONFIG to %d, wanted to set to %d" % ( + gyroConf, regVal) + return -2 + # @TODO Add proper error log def setRangeAccel(self, rangeVal): """ Sets the readout range for the accelerometer Possible values rangeVal == self.RANGE_GYRO_\16G\8G\4G\2G """ - regVal = rangeVal<<3 # Shift to bits [3:4] ACCEL_FS_SEL - self.writeRegister(self.REG_ACCEL_CONFIG1, regVal) - - accelConf = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] - # test if we did it right?? - if (regVal == accelConf): - # Update Current range variable - self.currentRangeAccel = rangeVal - else: - print "\n WARNING! FAILED TO SET acceletometer range!" - print "\n\tI've set REG_ACCEL_CONFIG to %d, wanted to set to %d" % ( - accelConf, regVal) - - # @TODO Add proper error log + # We only accept rangeVal = 0 to 3 + if (rangeVal < 0 or rangeVal > 3): + print "\n Invalid RANGE value! Range not changed" + # @TODO Error handling + return -1 + else: + + # Preserve previous REG bits + regOld = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] + regOld &= ~(0x3<<3) # Clear [3:4] FS bits + # Combine regOld and rangeVal shifted to bits [3:4] ACCEL_FS_SEL + regVal = regOld | (rangeVal<<3) + self.writeRegister(self.REG_ACCEL_CONFIG1, regVal) + + accelConf = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] + # test if we did it right?? + if (regVal == accelConf): + # Update Current range variable + self.currentRangeAccel = rangeVal + return 0 + else: + print "\n WARNING! FAILED TO SET acceletometer range!" + print "\n\tI've set REG_ACCEL_CONFIG to %d, wanted to set to %d" % ( + accelConf, regVal) + return -2 + # @TODO Add proper error log def getAccel(self): @@ -201,6 +231,14 @@ def runSelfTest(self): sumGyro = [0] * 200 STSumAccel = [0] * 200 STSumGyro = [0] * 200 + + + # Read configs for restoration + oldRegConf = self.readRegister(self.REG_CONFIG, 1)[0] + oldGyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] + oldAccelConf1 = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] + oldAccelConf2 = self.readRegister(self.REG_ACCEL_CONFIG2, 1)[0] + # Set range and sampling rate # Set gyro sample rate to 1 kHz @@ -260,9 +298,9 @@ def runSelfTest(self): STSumGyro[i] /= 200.0 - # Set things back to normal again - self.setRangeGyro(self.RANGE_GYRO_250DPS) # Write 0x00 to gyro config - self.setRangeAccel(self.RANGE_ACCEL_2G) # Write 0x00 to accel config + # Set things back to normal again (0x00 to gyro & accel config) + self.writeRegister(self.REG_ACCEL_CONFIG1, 0x00) + self.writeRegister(self.REG_GYRO_CONFIG, 0x00) # Retrieve factory self-test data STGyroX, STGyroY, STGyroZ = self.readRegister(0x00, 3) @@ -284,22 +322,16 @@ def runSelfTest(self): for i in range(3): devAccel[i] = 100.0 * ( STSumAccel[i] - sumAccel[i]) / FTrimAccel[i] devGyro[i] = 100.0 * ( STSumGyro[i] - sumGyro[i]) / FTrimGyro[i] - - return devAccel, devGyro - """ - // Configure the gyro and accelerometer for normal operation - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); - delay(25); // Delay a while to let the device stabilize - """ - """ - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< Date: Sun, 2 Aug 2015 17:58:13 +0800 Subject: [PATCH 09/22] moved magnetometer init to initMag(); inconsistent issues with mag hanging over I2C --- bbio/libraries/MPU9250/MPU9250.py | 79 ++++++++++++++++++++++++------- examples/MPU9250_test.py | 12 ++++- 2 files changed, 71 insertions(+), 20 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index a6a7e23..7274a23 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -8,14 +8,22 @@ See PyBBIO/LICENSE.txt """ import bbio +import time class MPU9250(object): - ID_VALUE = 0x71 # precoded identification string in WHOAMI Reg - - REG_ID = 0x75 # WHOAMI - REG_SMPLRT_DIV = 0x19 - REG_CONFIG = 0x18 + + # Set Register addresses + + REG_XG_OFFSET_H = 0x13 # User-defined trim values for gyroscope + REG_XG_OFFSET_L = 0x14 + REG_YG_OFFSET_H = 0x15 + REG_YG_OFFSET_L = 0x16 + REG_ZG_OFFSET_H = 0x17 + REG_ZG_OFFSET_L = 0x18 + + REG_SMPLRT_DIV = 0x19 # Sample rate divider + REG_CONFIG = 0x18 # General & Gyro Config REG_GYRO_CONFIG = 0x1B REG_ACCEL_CONFIG1 = 0x1C REG_ACCEL_CONFIG2 = 0x1D @@ -26,7 +34,10 @@ class MPU9250(object): REG_I2C_SLV0_REG = 0x26 REG_I2C_SLV0_CTRL = 0x27 REG_I2C_SLV0_DO = 0x63 + REG_USER_CTRL = 0x6A + REG_PWR_MGMT_1 = 0x6B # Device defaults to the SLEEP mode + REG_PWR_MGMT_2 = 0x6C AK8963_CNTL1 = 0x0A AK8963_CNTL2 = 0x0B @@ -34,7 +45,12 @@ class MPU9250(object): REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 - + REG_XA_OFFSET_H = 0x77 # User-defined trim values for accelerometer + REG_XA_OFFSET_L = 0x78 + REG_YA_OFFSET_H = 0x7A + REG_YA_OFFSET_L = 0x7B + REG_ZA_OFFSET_H = 0x7D + REG_ZA_OFFSET_L = 0x7E # Define possible gyro & accel ranges RANGE_GYRO_2000DPS = 3 @@ -50,6 +66,9 @@ class MPU9250(object): SCALE_GYRO = 250, 500, 1000, 2000 SCALE_ACCEL = 2, 4, 8, 16 + REG_ID = 0x75 # WHOAMI REG + MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG + def __init__(self, spi, cs=0): self.spi = spi self.cs = cs @@ -60,22 +79,23 @@ def __init__(self, spi, cs=0): # Am I talking to an MPU9250? id_val = self.readRegister(self.REG_ID)[0] # print "\nGot WHOAMI = 0x%02x" %id_val - assert id_val == self.ID_VALUE, "MPU9250 not detected on SPI bus" + assert id_val == self.MPU9250_ID_VALUE, "MPU9250 not detected on SPI bus" + + # @TODO Fix mysterious periodic hang of magnetometer ? + self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + time.sleep(0.2) + self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock source + self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON + time.sleep(0.2) + #This part is to set up the magnetometer, due to it being a separate device self.writeRegister(self.REG_I2C_MST_CTRL, 0x0D) # I2C speed 400 Khz - self.writeRegister(self.REG_USER_CTRL, 0x22) # + self.writeRegister(self.REG_USER_CTRL, 0x32) # + time.sleep(0.2) - # Reset magnetometer - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL2) - self.writeRegister(self.REG_I2C_SLV0_DO, 0x01) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - # Set 16-bit continues MODE1 readouts - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - + # Init magnetometer + self.initMag() # Reset gyro and accel configs self.writeRegister(self.REG_GYRO_CONFIG, 0x00) @@ -95,6 +115,22 @@ def __init__(self, spi, cs=0): # Done with init() + def initMag(self): + """ Initalize on-die AK8963 magnetometer & get offset""" + # Soft Reset + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL2) + self.writeRegister(self.REG_I2C_SLV0_DO, 0x01) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + time.sleep(0.1) # Stabilize + + # Set 16-bit output & continuous MODE1 + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + + + def ak8963Whoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) @@ -333,6 +369,13 @@ def runSelfTest(self): return devAccel, devGyro + def calibrateGyroAccel(self): + """ Calibration function for gyroscope and accelerometer """ + + biasGyro = [0] * 3 + biasAccel = [0] * 3 + + def readRegister(self, addr, n_bytes=1): """ Reads the value in the given register, or if the optional parameter 'n_bytes' is greater than 1 returns n_bytes register values starting diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index d565bd6..97fd9d9 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -19,7 +19,7 @@ def setup(): - delay(100) # Let the I2C reset settle + delay(200) # Let the I2C reset settle # Sanity check mpu.ak8963Whoami() @@ -33,6 +33,7 @@ def setup(): # Change it back to maximum mpu.setRangeGyro(mpu.RANGE_GYRO_2000DPS) + mpu.setRangeGyro(27) delay(1000) confData = mpu.readRegister( 27, 2) @@ -47,6 +48,9 @@ def setup(): print "\n\t ACCEL: %f %f %f" % (devAccel[0], devAccel[1], devAccel[2]) print "\n\t GYRO: %f %f %f" % (devGyro[0], devGyro[1], devGyro[2]) + + # mpu.calibrateGyroAccel() + delay(1000) confData = mpu.readRegister( 27, 2) print '\n GyroConfig: {:#010b}'.format(confData[0]) @@ -55,6 +59,10 @@ def setup(): def loop(): + + # mpu.calibrateGyroAccel() + + # Get data accelX, accelY, accelZ = mpu.getAccel() gyroX, gyroY, gyroZ = mpu.getGyro() @@ -69,7 +77,7 @@ def loop(): degC = mpu.getTemp() - print "\n On-die temperature is: %d C" % degC + print "\n On-die temperature: %d C" % degC delay(200) From c059e85646f879be8030f9d5e869fd8d8506939e Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Mon, 3 Aug 2015 22:02:21 +0800 Subject: [PATCH 10/22] added part of calibrateGyroAccel(); still needs fixing --- bbio/libraries/MPU9250/MPU9250.py | 326 ++++++++++++++++++++++++++++++ 1 file changed, 326 insertions(+) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 7274a23..e1c0e7a 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -27,6 +27,8 @@ class MPU9250(object): REG_GYRO_CONFIG = 0x1B REG_ACCEL_CONFIG1 = 0x1C REG_ACCEL_CONFIG2 = 0x1D + + REG_INT_ENABLE = 0x38 REG_FIFO_EN = 0x23 REG_I2C_MST_CTRL = 0x24 @@ -45,6 +47,10 @@ class MPU9250(object): REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 + REG_FIFO_COUNTH = 0x72 + REG_FIFO_COUNTL = 0x73 + REG_FIFO_R_W = 0x74 + REG_XA_OFFSET_H = 0x77 # User-defined trim values for accelerometer REG_XA_OFFSET_L = 0x78 REG_YA_OFFSET_H = 0x7A @@ -88,6 +94,12 @@ def __init__(self, spi, cs=0): self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON time.sleep(0.2) + + accelOffsetData = self.readRegister( self.REG_XA_OFFSET_H, 2) + print '\n AccelX_H: {:#010b}'.format(accelOffsetData[0]) + print '\n AccelX_L: {:#010b}'.format(accelOffsetData[1]) + + #This part is to set up the magnetometer, due to it being a separate device self.writeRegister(self.REG_I2C_MST_CTRL, 0x0D) # I2C speed 400 Khz @@ -374,7 +386,311 @@ def calibrateGyroAccel(self): biasGyro = [0] * 3 biasAccel = [0] * 3 + out = [0] * 6 + + self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + time.sleep(0.2) + self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock + self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON + time.sleep(0.2) + + #Configure device for bias calculation + self.writeRegister(self.REG_INT_ENABLE, 0x00) # Disable all interrupts + self.writeRegister(self.REG_FIFO_EN, 0x00) # Disable FIFO + self.writeRegister(self.REG_PWR_MGMT_1, 0x00) # Turn on internal clock + self.writeRegister(self.REG_I2C_MST_CTRL, 0x00) # Disable I2C master + self.writeRegister(self.REG_USER_CTRL, 0x00) # Disable FIFO & I2C MSTR + self.writeRegister(self.REG_USER_CTRL, 0x0C) # Reset FIFO and DMP + time.sleep(0.2) + + self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz + self.writeRegister(self.REG_SMPLRT_DIV, 0x00) # Set sample rate to 1 kHz + self.setRangeGyro(self.RANGE_GYRO_250DPS) # Set gyro full-scale to 250 degrees per second, maximum sensitivity + self.setRangeAccel(self.RANGE_ACCEL_2G) # Set accelerometer full-scale to 2 g, maximum sensitivity + + gyrosensitivity = 131 # = 131 LSB/degrees/sec + accelsensitivity = 16384 # = 16384 LSB/g + + + # Configure FIFO to capture accelerometer and gyro data for bias compute + self.writeRegister(self.REG_USER_CTRL, 0x40) # Enable FIFO + self.writeRegister(self.REG_FIFO_EN, 0x78) # Enable gyro and accelerometer sensors for FIFO (max size 512 bytes) + time.sleep(0.04) # accumulate 40 samples in 40 milliseconds = 480 bytes + + # At the end of sample accumulation, turn off FIFO sensor read + self.writeRegister(self.REG_FIFO_EN, 0x00) # Disable gyro and accelerometer sensors for FIFO + fifoData = self.readRegister(self.REG_FIFO_COUNTH, 2) # read FIFO sample count + fifoCount = fifoData[0]<<8 | fifoData[1] + packetCount = fifoCount/12 # How many sets of full gyro and accelerometer data for averaging + + for i in range(packetCount): + + tempAccel = [0] * 3 + tempGyro = [0] * 3 + bytes = self.readRegister(self.REG_FIFO_R_W, 12) # read bytes for averaging + + # Form signed 16-bit integer for each sample in FIFO + tempAccel[0] = self.fromSigned16(bytes[0:2]) + tempAccel[1] = self.fromSigned16(bytes[2:4]) + tempAccel[2] = self.fromSigned16(bytes[4:6]) + tempGyro[0] = self.fromSigned16(bytes[6:8]) + tempGyro[1] = self.fromSigned16(bytes[8:10]) + tempGyro[2] = self.fromSigned16(bytes[10:12]) + + + biasAccel[0] += tempAccel[0] # Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + biasAccel[1] += tempAccel[1] + biasAccel[2] += tempAccel[2] + biasGyro[0] += tempGyro[0] + biasGyro[1] += tempGyro[1] + biasGyro[2] += tempGyro[2] + + + # Normalize sums to get average count biases + biasAccel[0] /= packetCount + biasAccel[1] /= packetCount + biasAccel[2] /= packetCount + biasGyro[0] /= packetCount + biasGyro[1] /= packetCount + biasGyro[2] /= packetCount + + + print "\n packetCount: %d" %d packetCount + if(biasAccel[2] > 0.0): + # Remove Gravity from readings + biasAccel[2] -= accelsensitivity # -1 G + else: + biasAccel[2] += accelsensitivity # or +1 G + + # Construct Gyro biases for push to the hardware gyro bias registers + # They were reset to zero upon device hardware reset during startup + + # MULTIPLY by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + # Biases are additive, so change sign on calculated average gyro biases + data = [0] * 6 + data[0], data[1] = self.toSigned16( -1 * biasGyro[0]) + data[2], data[3] = self.toSigned16( -1 * biasGyro[1]) + data[4], data[5] = self.toSigned16( -1 * biasGyro[2]) + + # Push gyro biases to hardware registers + self.writeRegister(self.REG_XG_OFFSET_H, data[0]) + self.writeRegister(self.REG_XG_OFFSET_L, data[1]) + self.writeRegister(self.REG_YG_OFFSET_H, data[2]) + self.writeRegister(self.REG_YG_OFFSET_L, data[3]) + self.writeRegister(self.REG_ZG_OFFSET_H, data[4]) + self.writeRegister(self.REG_ZG_OFFSET_L, data[5]) + + # Output scaled gyro biases for display in the main program + out[3] = biasGyro[0]/ gyrosensitivity + out[4] = biasGyro[1]/ gyrosensitivity + out[5] = biasGyro[2]/ gyrosensitivity + + + + #Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain + # factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold + # non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature + # compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that + # the accelerometer biases calculated above must be divided by 8. + + biasAccelReg = [0] * 3 # Hold the factory accelerometer trim biases + # Read factory accelerometer trim values (!15 bit!) + bytes = self.readRegister(self.REG_XA_OFFSET_H, 6) + biasAccelReg[0] = self.fromSigned16( bytes[0:2]) / 2 + biasAccelReg[0] = self.fromSigned16( bytes[2:4]) / 2 + biasAccelReg[0] = self.fromSigned16( bytes[4:6]) / 2 + # Since we're interested in the top 15 bits, we devide by 2 for the 16bit + # signed number. Conveniently this also ignores bit 0 completely + + + # Check for reserved bits (temperature compensation?) & preserve + bitMask = [0] * 3 + bitMask[0] = 0x01 & bytes[1] + bitMask[1] = 0x01 & bytes[3] + bitMask[2] = 0x01 & bytes[5] + + # Construct total accelerometer bias, including calculated average accelerometer bias from above + # bias Avg is calculated in +-2Gs / 16-bit two's compelment = 16384 Gs/LSB + # bias in Regs is saved as +-16Gs / 15-bit two's complement = 1024 Gs/LSB + biasAccelReg[0] -= (biasAccel[0]/16) # Subtract avg bias; in proper scale + biasAccelReg[1] -= (biasAccel[1]/16) + biasAccelReg[2] -= (biasAccel[2]/16) + + data[0], data[1] = self.toSigned16(biasAccel[0] * 2) # equal to shift left + data[2], data[3] = self.toSigned16(biasAccel[1] * 2) + data[4], data[5] = self.toSigned16(biasAccel[2] * 2) + data[1] = data[1] | bitMask[0] # preserve reserved bit + data[3] = data[3] | bitMask[1] + data[5] = data[5] | bitMask[2] + + + #Apparently this is not working for the acceleration biases in the MPU-9250 + #Are we handling the temperature correction bit properly? + + #Push accelerometer biases to hardware registers + self.writeRegister(self.REG_XA_OFFSET_H, data[0]) + self.writeRegister(self.REG_XA_OFFSET_L, data[1]) + self.writeRegister(self.REG_YA_OFFSET_H, data[2]) + self.writeRegister(self.REG_YA_OFFSET_L, data[3]) + self.writeRegister(self.REG_ZA_OFFSET_H, data[4]) + self.writeRegister(self.REG_ZA_OFFSET_L, data[5]) + + #Output scaled accelerometer biases for display in the main program + out[3] = biasAccel[0]/accelsensitivity + out[4] = biasAccel[1]/accelsensitivity + out[5] = biasAccel[2]/accelsensitivity + + + return out + + + + """ + + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + + // reset device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device + delay(100); + + // get stable time source; Auto select clock source to be PLL gyroscope reference if ready + // else use the internal oscillator, bits 2:0 = 001 + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); + delay(200); + +// Configure device for bias calculation + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP + delay(15); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g + + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + + for (ii = 0; ii < packet_count; ii++) { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; + accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; + gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; + gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; + gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + gyro_bias[0] += (int32_t) gyro_temp[0]; + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + +} + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + accel_bias[1] /= (int32_t) packet_count; + accel_bias[2] /= (int32_t) packet_count; + gyro_bias[0] /= (int32_t) packet_count; + gyro_bias[1] /= (int32_t) packet_count; + gyro_bias[2] /= (int32_t) packet_count; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; + data[3] = (-gyro_bias[1]/4) & 0xFF; + data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; + data[5] = (-gyro_bias[2]/4) & 0xFF; + +// Push gyro biases to hardware registers + writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); + +// Output scaled gyro biases for display in the main program + dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; + +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + +// Apparently this is not working for the acceleration biases in the MPU-9250 +// Are we handling the temperature correction bit properly? +// Push accelerometer biases to hardware registers + writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); + +// Output scaled accelerometer biases for display in the main program + dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; + dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; + dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; + """ def readRegister(self, addr, n_bytes=1): """ Reads the value in the given register, or if the optional parameter @@ -399,4 +715,14 @@ def fromSigned16(self, bytes): x = self.fromUnsigned16(bytes) if x > 32767: return -(65536-x) return x + + def toUnsigned16(self, int): + """ Convert unsigned short int to register values """ + return [(int >> 8) & 0xFF, int & 0xFF] + + + def toSigned16(self, int): + """ Convert signed short int to register values""" + if int < 0: return self.toUnsigned16(65536 + int ) # int is negative + return self.toUnsigned16(int) From d7ddf273b8ddd3d6cb3c23956be5e089ba7d6eb8 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Mon, 3 Aug 2015 22:04:21 +0800 Subject: [PATCH 11/22] minor fix test print messages; still fixing calibrateGyroAccel() --- bbio/libraries/MPU9250/MPU9250.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index e1c0e7a..68b1cdb 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -456,7 +456,10 @@ def calibrateGyroAccel(self): biasGyro[2] /= packetCount - print "\n packetCount: %d" %d packetCount + print "\n packetCount: %d" % packetCount + print "\n ACCEL BIAS: %f %f %f\n GYRO BIAS: %f %f %f " % (biasAccel[0], biasAccel[1], biasAccel[2], biasGyro[0], biasGyro[1], biasGyro[2]) + + if(biasAccel[2] > 0.0): # Remove Gravity from readings biasAccel[2] -= accelsensitivity # -1 G From 582dedf8a3cebceddc39381babe4a0d0ae7bab00 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Wed, 5 Aug 2015 23:40:59 +0800 Subject: [PATCH 12/22] still working on calibrateGyroAccel() --- bbio/libraries/MPU9250/MPU9250.py | 97 +++++++++++++++++++++++-------- 1 file changed, 73 insertions(+), 24 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 68b1cdb..3172c38 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -387,8 +387,24 @@ def calibrateGyroAccel(self): biasGyro = [0] * 3 biasAccel = [0] * 3 out = [0] * 6 + + + # Read configs for restoration + oldSampleDiv = self.readRegister(self.REG_SMPLRT_DIV, 1)[0] + oldRegConf = self.readRegister(self.REG_CONFIG, 1)[0] + oldGyroConf = self.readRegister(self.REG_GYRO_CONFIG, 1)[0] + oldAccelConf1 = self.readRegister(self.REG_ACCEL_CONFIG1, 1)[0] + oldAccelConf2 = self.readRegister(self.REG_ACCEL_CONFIG2, 1)[0] + oldI2CControl = self.readRegister(self.REG_I2C_MST_CTRL, 1)[0] + oldUserControl = self.readRegister(self.REG_USER_CTRL, 1)[0] + + time.sleep(1) + confData = self.readRegister( 27, 2) + print '\n GyroConfig: {:#010b}'.format(confData[0]) + print '\n AccelConfig: {:#010b}'.format(confData[1]) - self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + + # self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers time.sleep(0.2) self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON @@ -403,10 +419,12 @@ def calibrateGyroAccel(self): self.writeRegister(self.REG_USER_CTRL, 0x0C) # Reset FIFO and DMP time.sleep(0.2) - self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz + # self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz self.writeRegister(self.REG_SMPLRT_DIV, 0x00) # Set sample rate to 1 kHz - self.setRangeGyro(self.RANGE_GYRO_250DPS) # Set gyro full-scale to 250 degrees per second, maximum sensitivity - self.setRangeAccel(self.RANGE_ACCEL_2G) # Set accelerometer full-scale to 2 g, maximum sensitivity + # self.setRangeGyro(self.RANGE_GYRO_250DPS) # Set gyro full-scale to 250 degrees per second, maximum sensitivity + # self.setRangeAccel(self.RANGE_ACCEL_2G) # Set accelerometer full-scale to 2 g, maximum sensitivity + self.writeRegister(self.REG_GYRO_CONFIG, 0x00) # Set gyro full-scale to 250 degrees per second, maximum sensitivity + self.writeRegister(self.REG_ACCEL_CONFIG1, 0x00) # Set accelerometer full-scale to 2 g, maximum sensitivity gyrosensitivity = 131 # = 131 LSB/degrees/sec accelsensitivity = 16384 # = 16384 LSB/g @@ -428,7 +446,10 @@ def calibrateGyroAccel(self): tempAccel = [0] * 3 tempGyro = [0] * 3 - bytes = self.readRegister(self.REG_FIFO_R_W, 12) # read bytes for averaging + + bytes = [0] * 12 + for j in range(12): + bytes[j] = self.readRegister(self.REG_FIFO_R_W, 1)[0] # read bytes for averaging # Form signed 16-bit integer for each sample in FIFO tempAccel[0] = self.fromSigned16(bytes[0:2]) @@ -457,14 +478,14 @@ def calibrateGyroAccel(self): print "\n packetCount: %d" % packetCount - print "\n ACCEL BIAS: %f %f %f\n GYRO BIAS: %f %f %f " % (biasAccel[0], biasAccel[1], biasAccel[2], biasGyro[0], biasGyro[1], biasGyro[2]) + print "\n ACCEL BIAS: %d %d %d\n GYRO BIAS: %d %d %d " % (biasAccel[0], biasAccel[1], biasAccel[2], biasGyro[0], biasGyro[1], biasGyro[2]) - if(biasAccel[2] > 0.0): + #if(biasAccel[2] > 0): # Remove Gravity from readings - biasAccel[2] -= accelsensitivity # -1 G - else: - biasAccel[2] += accelsensitivity # or +1 G + # biasAccel[2] -= accelsensitivity # -1 G + #else: + # biasAccel[2] += accelsensitivity # or +1 G # Construct Gyro biases for push to the hardware gyro bias registers # They were reset to zero upon device hardware reset during startup @@ -475,7 +496,8 @@ def calibrateGyroAccel(self): data[0], data[1] = self.toSigned16( -1 * biasGyro[0]) data[2], data[3] = self.toSigned16( -1 * biasGyro[1]) data[4], data[5] = self.toSigned16( -1 * biasGyro[2]) - + + # Push gyro biases to hardware registers self.writeRegister(self.REG_XG_OFFSET_H, data[0]) self.writeRegister(self.REG_XG_OFFSET_L, data[1]) @@ -485,9 +507,9 @@ def calibrateGyroAccel(self): self.writeRegister(self.REG_ZG_OFFSET_L, data[5]) # Output scaled gyro biases for display in the main program - out[3] = biasGyro[0]/ gyrosensitivity - out[4] = biasGyro[1]/ gyrosensitivity - out[5] = biasGyro[2]/ gyrosensitivity + out[0] = float(biasGyro[0]) / gyrosensitivity + out[1] = float(biasGyro[1]) / gyrosensitivity + out[2] = float(biasGyro[2]) / gyrosensitivity @@ -499,10 +521,18 @@ def calibrateGyroAccel(self): biasAccelReg = [0] * 3 # Hold the factory accelerometer trim biases # Read factory accelerometer trim values (!15 bit!) - bytes = self.readRegister(self.REG_XA_OFFSET_H, 6) + # THEY ARE NOT IN AN ADDRESS SEQUENCE OF 6 BYTES!!! + # Not this -> bytes = self.readRegister(self.REG_XA_OFFSET_H, 6) + # Addresses: + # XA_H|XA_L = 0x77|0x78; YA_H|YA_L = 0x7A|0x7B; ZA_H|ZA_L = 0x7D|0x7E + bytes = [0] * 6 + bytes[0], bytes[1] = self.readRegister(self.REG_XA_OFFSET_H, 2) + bytes[2], bytes[3] = self.readRegister(self.REG_YA_OFFSET_H, 2) + bytes[4], bytes[5] = self.readRegister(self.REG_ZA_OFFSET_H, 2) + print "\n accel OFFSET : %s" % bytes biasAccelReg[0] = self.fromSigned16( bytes[0:2]) / 2 - biasAccelReg[0] = self.fromSigned16( bytes[2:4]) / 2 - biasAccelReg[0] = self.fromSigned16( bytes[4:6]) / 2 + biasAccelReg[1] = self.fromSigned16( bytes[2:4]) / 2 + biasAccelReg[2] = self.fromSigned16( bytes[4:6]) / 2 # Since we're interested in the top 15 bits, we devide by 2 for the 16bit # signed number. Conveniently this also ignores bit 0 completely @@ -520,13 +550,15 @@ def calibrateGyroAccel(self): biasAccelReg[1] -= (biasAccel[1]/16) biasAccelReg[2] -= (biasAccel[2]/16) - data[0], data[1] = self.toSigned16(biasAccel[0] * 2) # equal to shift left - data[2], data[3] = self.toSigned16(biasAccel[1] * 2) - data[4], data[5] = self.toSigned16(biasAccel[2] * 2) + data[0], data[1] = self.toSigned16(biasAccelReg[0] * 2) + data[2], data[3] = self.toSigned16(biasAccelReg[1] * 2) + data[4], data[5] = self.toSigned16(biasAccelReg[2] * 2) + # Multiply by 2 equals to shift left data[1] = data[1] | bitMask[0] # preserve reserved bit data[3] = data[3] | bitMask[1] data[5] = data[5] | bitMask[2] + print "\n accel OFFSET out : %s" % data #Apparently this is not working for the acceleration biases in the MPU-9250 #Are we handling the temperature correction bit properly? @@ -539,12 +571,29 @@ def calibrateGyroAccel(self): self.writeRegister(self.REG_ZA_OFFSET_H, data[4]) self.writeRegister(self.REG_ZA_OFFSET_L, data[5]) + time.sleep(0.2) + bytes[0], bytes[1] = self.readRegister(self.REG_XA_OFFSET_H, 2) + bytes[2], bytes[3] = self.readRegister(self.REG_YA_OFFSET_H, 2) + bytes[4], bytes[5] = self.readRegister(self.REG_ZA_OFFSET_H, 2) + print "\n accel OFFSET OUT! : %s" % bytes + #Output scaled accelerometer biases for display in the main program - out[3] = biasAccel[0]/accelsensitivity - out[4] = biasAccel[1]/accelsensitivity - out[5] = biasAccel[2]/accelsensitivity - + out[3] = float(biasAccel[0]) / accelsensitivity + out[4] = float(biasAccel[1]) / accelsensitivity + out[5] = float(biasAccel[2]) / accelsensitivity + + # Restore old configuration registers + self.writeRegister(self.REG_SMPLRT_DIV, oldSampleDiv) + self.writeRegister(self.REG_I2C_MST_CTRL, oldI2CControl) + self.writeRegister(self.REG_USER_CTRL, oldUserControl) + self.writeRegister(self.REG_CONFIG, oldRegConf) + self.writeRegister(self.REG_GYRO_CONFIG, oldGyroConf) + self.writeRegister(self.REG_ACCEL_CONFIG1, oldAccelConf1) + self.writeRegister(self.REG_ACCEL_CONFIG2, oldAccelConf2) + + # Set scale properly + return out From 5863dc832c7b4bc364d0eb79e37f402889a61108 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 6 Aug 2015 01:25:11 +0800 Subject: [PATCH 13/22] moved current test; cleaning later --- examples/MPU9250_test.py | 21 +++++++++++++++++++-- 1 file changed, 19 insertions(+), 2 deletions(-) diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 97fd9d9..622e5e2 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -33,6 +33,7 @@ def setup(): # Change it back to maximum mpu.setRangeGyro(mpu.RANGE_GYRO_2000DPS) + # Can we mess it up? mpu.setRangeGyro(27) delay(1000) @@ -49,19 +50,35 @@ def setup(): print "\n\t GYRO: %f %f %f" % (devGyro[0], devGyro[1], devGyro[2]) - # mpu.calibrateGyroAccel() + data = mpu.calibrateGyroAccel() + + print "\n GRYO BIAS: %f %f %f\n ACCEL BIAS: %f %f %f " % (data[0],data[1],data[2],data[3],data[4],data[5]) + delay(1000) confData = mpu.readRegister( 27, 2) print '\n GyroConfig: {:#010b}'.format(confData[0]) print '\n AccelConfig: {:#010b}'.format(confData[1]) delay(1000) - + + + accelOffsetData = mpu.readRegister( 119, 2) + print '\n AccelX_H: {:#010b}'.format(accelOffsetData[0]) + print '\n AccelX_L: {:#010b}'.format(accelOffsetData[1]) + + scale = mpu.currentRangeAccel + print "\n ACCEL RANGE = %d" % mpu.SCALE_ACCEL[scale] + + mpu.currentRangeAccel = 3 + mpu.currentRangeGyro = 3 def loop(): # mpu.calibrateGyroAccel() + confData = mpu.readRegister( 27, 2) + print '\n GyroConfig: {:#010b}'.format(confData[0]) + print '\n AccelConfig: {:#010b}'.format(confData[1]) # Get data accelX, accelY, accelZ = mpu.getAccel() From 08a5c9a6ece01d5e357e90477f5fc76100c02728 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 6 Aug 2015 03:19:41 +0800 Subject: [PATCH 14/22] FIFO fixed; calibrateAccelGyro() works like a charm --- bbio/libraries/MPU9250/MPU9250.py | 273 ++++++++---------------------- examples/MPU9250_test.py | 27 ++- 2 files changed, 79 insertions(+), 221 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 3172c38..632e4c4 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -12,7 +12,9 @@ class MPU9250(object): - + # Are we for real? + dryRun = 0 + # Set Register addresses REG_XG_OFFSET_H = 0x13 # User-defined trim values for gyroscope @@ -381,8 +383,10 @@ def runSelfTest(self): return devAccel, devGyro - def calibrateGyroAccel(self): - """ Calibration function for gyroscope and accelerometer """ + def calibrateAccelGyro(self): + """ Calibration function for gyroscope and accelerometer + Returns accel biases (x,y,z) in Gs & Gyro biases (x,y,z) in DPS + """ biasGyro = [0] * 3 biasAccel = [0] * 3 @@ -397,20 +401,14 @@ def calibrateGyroAccel(self): oldAccelConf2 = self.readRegister(self.REG_ACCEL_CONFIG2, 1)[0] oldI2CControl = self.readRegister(self.REG_I2C_MST_CTRL, 1)[0] oldUserControl = self.readRegister(self.REG_USER_CTRL, 1)[0] - - time.sleep(1) - confData = self.readRegister( 27, 2) - print '\n GyroConfig: {:#010b}'.format(confData[0]) - print '\n AccelConfig: {:#010b}'.format(confData[1]) - - # self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers time.sleep(0.2) self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON time.sleep(0.2) - #Configure device for bias calculation + # Configure device for bias calculation self.writeRegister(self.REG_INT_ENABLE, 0x00) # Disable all interrupts self.writeRegister(self.REG_FIFO_EN, 0x00) # Disable FIFO self.writeRegister(self.REG_PWR_MGMT_1, 0x00) # Turn on internal clock @@ -419,7 +417,7 @@ def calibrateGyroAccel(self): self.writeRegister(self.REG_USER_CTRL, 0x0C) # Reset FIFO and DMP time.sleep(0.2) - # self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz + self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz self.writeRegister(self.REG_SMPLRT_DIV, 0x00) # Set sample rate to 1 kHz # self.setRangeGyro(self.RANGE_GYRO_250DPS) # Set gyro full-scale to 250 degrees per second, maximum sensitivity # self.setRangeAccel(self.RANGE_ACCEL_2G) # Set accelerometer full-scale to 2 g, maximum sensitivity @@ -441,15 +439,21 @@ def calibrateGyroAccel(self): fifoData = self.readRegister(self.REG_FIFO_COUNTH, 2) # read FIFO sample count fifoCount = fifoData[0]<<8 | fifoData[1] packetCount = fifoCount/12 # How many sets of full gyro and accelerometer data for averaging - + + # print " EXTRAS: %d" % (fifoCount % 12) + # ALIGN FIFO: Read extra bits in buffer (and remove them)!!! + for j in range(fifoCount % 12): + self.readRegister(self.REG_FIFO_R_W, 1)[0] + for i in range(packetCount): tempAccel = [0] * 3 tempGyro = [0] * 3 - + bytes = [0] * 12 + # read bytes for averaging for j in range(12): - bytes[j] = self.readRegister(self.REG_FIFO_R_W, 1)[0] # read bytes for averaging + bytes[j] = self.readRegister(self.REG_FIFO_R_W, 1)[0] # Form signed 16-bit integer for each sample in FIFO tempAccel[0] = self.fromSigned16(bytes[0:2]) @@ -459,8 +463,10 @@ def calibrateGyroAccel(self): tempGyro[1] = self.fromSigned16(bytes[8:10]) tempGyro[2] = self.fromSigned16(bytes[10:12]) - - biasAccel[0] += tempAccel[0] # Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + # print "\n ACCEL BIAS: %d %d %d\n GYRO BIAS: %d %d %d " % (tempAccel[0], tempAccel[1], tempAccel[2], tempGyro[0], tempGyro[1], tempGyro[2]) + + # Sum individual signed 16-bit biases + biasAccel[0] += tempAccel[0] biasAccel[1] += tempAccel[1] biasAccel[2] += tempAccel[2] biasGyro[0] += tempGyro[0] @@ -477,10 +483,13 @@ def calibrateGyroAccel(self): biasGyro[2] /= packetCount - print "\n packetCount: %d" % packetCount - print "\n ACCEL BIAS: %d %d %d\n GYRO BIAS: %d %d %d " % (biasAccel[0], biasAccel[1], biasAccel[2], biasGyro[0], biasGyro[1], biasGyro[2]) + # print "\n packetCount: %d" % packetCount + # print "\n ACCEL BIAS: %d %d %d\n GYRO BIAS: %d %d %d " % (biasAccel[0], biasAccel[1], biasAccel[2], biasGyro[0], biasGyro[1], biasGyro[2]) + + # OPTIONAL: Do you need this? + # We don't want to preserve 1G on Z axis, we don't use that in space #if(biasAccel[2] > 0): # Remove Gravity from readings # biasAccel[2] -= accelsensitivity # -1 G @@ -490,36 +499,38 @@ def calibrateGyroAccel(self): # Construct Gyro biases for push to the hardware gyro bias registers # They were reset to zero upon device hardware reset during startup - # MULTIPLY by 4 to get 32.9 LSB per deg/s to conform to expected bias input format - # Biases are additive, so change sign on calculated average gyro biases + # DIVIDE by 4 to get 32.9 LSB/DPS to conform to expected bias input format + # Biases are additive, so change sign on calculated average gyro biases data = [0] * 6 - data[0], data[1] = self.toSigned16( -1 * biasGyro[0]) - data[2], data[3] = self.toSigned16( -1 * biasGyro[1]) - data[4], data[5] = self.toSigned16( -1 * biasGyro[2]) + data[0], data[1] = self.toSigned16( -1 * biasGyro[0] / 4) + data[2], data[3] = self.toSigned16( -1 * biasGyro[1] / 4) + data[4], data[5] = self.toSigned16( -1 * biasGyro[2] / 4) + # Push gyro biases to hardware registers + if(not self.dryRun): + self.writeRegister(self.REG_XG_OFFSET_H, data[0]) + self.writeRegister(self.REG_XG_OFFSET_L, data[1]) + self.writeRegister(self.REG_YG_OFFSET_H, data[2]) + self.writeRegister(self.REG_YG_OFFSET_L, data[3]) + self.writeRegister(self.REG_ZG_OFFSET_H, data[4]) + self.writeRegister(self.REG_ZG_OFFSET_L, data[5]) - # Push gyro biases to hardware registers - self.writeRegister(self.REG_XG_OFFSET_H, data[0]) - self.writeRegister(self.REG_XG_OFFSET_L, data[1]) - self.writeRegister(self.REG_YG_OFFSET_H, data[2]) - self.writeRegister(self.REG_YG_OFFSET_L, data[3]) - self.writeRegister(self.REG_ZG_OFFSET_H, data[4]) - self.writeRegister(self.REG_ZG_OFFSET_L, data[5]) - - # Output scaled gyro biases for display in the main program - out[0] = float(biasGyro[0]) / gyrosensitivity - out[1] = float(biasGyro[1]) / gyrosensitivity - out[2] = float(biasGyro[2]) / gyrosensitivity + # Output scaled gyro biases for display in the main program + out[3] = float(biasGyro[0]) / gyrosensitivity + out[4] = float(biasGyro[1]) / gyrosensitivity + out[5] = float(biasGyro[2]) / gyrosensitivity - #Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain - # factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold - # non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature - # compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that - # the accelerometer biases calculated above must be divided by 8. + # Construct the accelerometer biases for push to the hardware accelerometer + # bias registers. These registers contain factory trim values which must + # be added to the calculated accelerometer biases; on boot up these + # registers will hold non-zero values. + # In addition, bit 0 of the lower byte must be preserved since it is used + # for temperature compensation (?) calculations. Accelerometer bias + # registers expect bias input as 1024 LSB per g, so that the accelerometer + # biases calculated above must be divided by 16. - biasAccelReg = [0] * 3 # Hold the factory accelerometer trim biases # Read factory accelerometer trim values (!15 bit!) # THEY ARE NOT IN AN ADDRESS SEQUENCE OF 6 BYTES!!! # Not this -> bytes = self.readRegister(self.REG_XA_OFFSET_H, 6) @@ -529,14 +540,14 @@ def calibrateGyroAccel(self): bytes[0], bytes[1] = self.readRegister(self.REG_XA_OFFSET_H, 2) bytes[2], bytes[3] = self.readRegister(self.REG_YA_OFFSET_H, 2) bytes[4], bytes[5] = self.readRegister(self.REG_ZA_OFFSET_H, 2) - print "\n accel OFFSET : %s" % bytes + # print "\n accel OFFSET : %s" % bytes + biasAccelReg = [0] * 3 # Hold the factory accelerometer trim biases biasAccelReg[0] = self.fromSigned16( bytes[0:2]) / 2 biasAccelReg[1] = self.fromSigned16( bytes[2:4]) / 2 biasAccelReg[2] = self.fromSigned16( bytes[4:6]) / 2 # Since we're interested in the top 15 bits, we devide by 2 for the 16bit # signed number. Conveniently this also ignores bit 0 completely - # Check for reserved bits (temperature compensation?) & preserve bitMask = [0] * 3 bitMask[0] = 0x01 & bytes[1] @@ -558,29 +569,28 @@ def calibrateGyroAccel(self): data[3] = data[3] | bitMask[1] data[5] = data[5] | bitMask[2] - print "\n accel OFFSET out : %s" % data + # print "\n accel OFFSET out : %s" % data - #Apparently this is not working for the acceleration biases in the MPU-9250 - #Are we handling the temperature correction bit properly? #Push accelerometer biases to hardware registers - self.writeRegister(self.REG_XA_OFFSET_H, data[0]) - self.writeRegister(self.REG_XA_OFFSET_L, data[1]) - self.writeRegister(self.REG_YA_OFFSET_H, data[2]) - self.writeRegister(self.REG_YA_OFFSET_L, data[3]) - self.writeRegister(self.REG_ZA_OFFSET_H, data[4]) - self.writeRegister(self.REG_ZA_OFFSET_L, data[5]) + if(not self.dryRun): + self.writeRegister(self.REG_XA_OFFSET_H, data[0]) + self.writeRegister(self.REG_XA_OFFSET_L, data[1]) + self.writeRegister(self.REG_YA_OFFSET_H, data[2]) + self.writeRegister(self.REG_YA_OFFSET_L, data[3]) + self.writeRegister(self.REG_ZA_OFFSET_H, data[4]) + self.writeRegister(self.REG_ZA_OFFSET_L, data[5]) time.sleep(0.2) bytes[0], bytes[1] = self.readRegister(self.REG_XA_OFFSET_H, 2) bytes[2], bytes[3] = self.readRegister(self.REG_YA_OFFSET_H, 2) bytes[4], bytes[5] = self.readRegister(self.REG_ZA_OFFSET_H, 2) - print "\n accel OFFSET OUT! : %s" % bytes + # print "\n accel OFFSET OUT! : %s" % bytes #Output scaled accelerometer biases for display in the main program - out[3] = float(biasAccel[0]) / accelsensitivity - out[4] = float(biasAccel[1]) / accelsensitivity - out[5] = float(biasAccel[2]) / accelsensitivity + out[0] = float(biasAccel[0]) / accelsensitivity + out[1] = float(biasAccel[1]) / accelsensitivity + out[2] = float(biasAccel[2]) / accelsensitivity # Restore old configuration registers @@ -593,157 +603,10 @@ def calibrateGyroAccel(self): self.writeRegister(self.REG_ACCEL_CONFIG2, oldAccelConf2) # Set scale properly - + return out - - """ - - uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data - uint16_t ii, packet_count, fifo_count; - int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; - - // reset device - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device - delay(100); - - // get stable time source; Auto select clock source to be PLL gyroscope reference if ready - // else use the internal oscillator, bits 2:0 = 001 - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); - writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); - delay(200); - -// Configure device for bias calculation - writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO - writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source - writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP - delay(15); - -// Configure MPU6050 gyro and accelerometer for bias calculation - writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz - writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz - writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity - writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity - - uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec - uint16_t accelsensitivity = 16384; // = 16384 LSB/g - - // Configure FIFO to capture accelerometer and gyro data for bias calculation - writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) - delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes - -// At end of sample accumulation, turn off FIFO sensor read - writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO - readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count - fifo_count = ((uint16_t)data[0] << 8) | data[1]; - packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging - - for (ii = 0; ii < packet_count; ii++) { - int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; - readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging - accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO - accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ; - accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ; - gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ; - gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ; - gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ; - - accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases - accel_bias[1] += (int32_t) accel_temp[1]; - accel_bias[2] += (int32_t) accel_temp[2]; - gyro_bias[0] += (int32_t) gyro_temp[0]; - gyro_bias[1] += (int32_t) gyro_temp[1]; - gyro_bias[2] += (int32_t) gyro_temp[2]; - -} - accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases - accel_bias[1] /= (int32_t) packet_count; - accel_bias[2] /= (int32_t) packet_count; - gyro_bias[0] /= (int32_t) packet_count; - gyro_bias[1] /= (int32_t) packet_count; - gyro_bias[2] /= (int32_t) packet_count; - - if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation - else {accel_bias[2] += (int32_t) accelsensitivity;} - -// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup - data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format - data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases - data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; - data[3] = (-gyro_bias[1]/4) & 0xFF; - data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; - data[5] = (-gyro_bias[2]/4) & 0xFF; - -// Push gyro biases to hardware registers - writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); - -// Output scaled gyro biases for display in the main program - dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity; - dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity; - dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity; - -// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain -// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold -// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature -// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that -// the accelerometer biases calculated above must be divided by 8. - - int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases - readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values - accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); - accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); - accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); - - uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers - uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis - - for(ii = 0; ii < 3; ii++) { - if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit - } - - // Construct total accelerometer bias, including calculated average accelerometer bias from above - accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) - accel_bias_reg[1] -= (accel_bias[1]/8); - accel_bias_reg[2] -= (accel_bias[2]/8); - - data[0] = (accel_bias_reg[0] >> 8) & 0xFF; - data[1] = (accel_bias_reg[0]) & 0xFF; - data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[2] = (accel_bias_reg[1] >> 8) & 0xFF; - data[3] = (accel_bias_reg[1]) & 0xFF; - data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers - data[4] = (accel_bias_reg[2] >> 8) & 0xFF; - data[5] = (accel_bias_reg[2]) & 0xFF; - data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers - -// Apparently this is not working for the acceleration biases in the MPU-9250 -// Are we handling the temperature correction bit properly? -// Push accelerometer biases to hardware registers - writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); - writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); - writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); - writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); - -// Output scaled accelerometer biases for display in the main program - dest2[0] = (float)accel_bias[0]/(float)accelsensitivity; - dest2[1] = (float)accel_bias[1]/(float)accelsensitivity; - dest2[2] = (float)accel_bias[2]/(float)accelsensitivity; - """ - def readRegister(self, addr, n_bytes=1): """ Reads the value in the given register, or if the optional parameter 'n_bytes' is greater than 1 returns n_bytes register values starting diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 622e5e2..06d7126 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -26,7 +26,7 @@ def setup(): # Change gyro range for fun mpu.setRangeGyro(mpu.RANGE_GYRO_500DPS) - delay(1000) + delay(500) confData = mpu.readRegister( 27, 2) print '\n GyroConfig: {:#010b}'.format(confData[0]) print '\n AccelConfig: {:#010b}'.format(confData[1]) @@ -36,11 +36,10 @@ def setup(): # Can we mess it up? mpu.setRangeGyro(27) - delay(1000) + delay(500) confData = mpu.readRegister( 27, 2) print '\n GyroConfig: {:#010b}'.format(confData[0]) print '\n AccelConfig: {:#010b}'.format(confData[1]) - # Do a selftest before we start devAccel, devGyro = mpu.runSelfTest() @@ -49,17 +48,19 @@ def setup(): print "\n\t ACCEL: %f %f %f" % (devAccel[0], devAccel[1], devAccel[2]) print "\n\t GYRO: %f %f %f" % (devGyro[0], devGyro[1], devGyro[2]) + # Calibrate Gyro & Accelerometer sensors + data = mpu.calibrateAccelGyro() - data = mpu.calibrateGyroAccel() - - print "\n GRYO BIAS: %f %f %f\n ACCEL BIAS: %f %f %f " % (data[0],data[1],data[2],data[3],data[4],data[5]) + print "\n Sensor offset (bias) values:" + print "\n\t ACCEL BIAS: %f %f %f\n\n\t GYRO BIAS: %f %f %f " % (data[0],data[1],data[2],data[3],data[4],data[5]) - - delay(1000) + + # Did we fix everything when we're out of the calibration + delay(500) confData = mpu.readRegister( 27, 2) - print '\n GyroConfig: {:#010b}'.format(confData[0]) print '\n AccelConfig: {:#010b}'.format(confData[1]) - delay(1000) + print '\n GyroConfig: {:#010b}'.format(confData[0]) + delay(500) accelOffsetData = mpu.readRegister( 119, 2) @@ -74,12 +75,6 @@ def setup(): def loop(): - # mpu.calibrateGyroAccel() - - confData = mpu.readRegister( 27, 2) - print '\n GyroConfig: {:#010b}'.format(confData[0]) - print '\n AccelConfig: {:#010b}'.format(confData[1]) - # Get data accelX, accelY, accelZ = mpu.getAccel() gyroX, gyroY, gyroZ = mpu.getGyro() From 67b5a4b32377606f2a7ccf4e44d7c9c0d12fdf24 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 6 Aug 2015 09:55:24 +0800 Subject: [PATCH 15/22] added setMagRate() --- bbio/libraries/MPU9250/MPU9250.py | 89 ++++++++++++++++++++++++++++--- examples/MPU9250_test.py | 16 +++++- 2 files changed, 96 insertions(+), 9 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 632e4c4..a98f02e 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -37,7 +37,7 @@ class MPU9250(object): REG_I2C_SLV0_ADDR = 0x25 REG_I2C_SLV0_REG = 0x26 REG_I2C_SLV0_CTRL = 0x27 - REG_I2C_SLV0_DO = 0x63 + REG_I2C_SLV0_DO = 0x63 # only for writing to SLV0; DO = DATA OUT REG_USER_CTRL = 0x6A REG_PWR_MGMT_1 = 0x6B # Device defaults to the SLEEP mode @@ -49,6 +49,8 @@ class MPU9250(object): REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 + REG_EXT_SENS_DATA_00 = 0x49 + REG_FIFO_COUNTH = 0x72 REG_FIFO_COUNTL = 0x73 REG_FIFO_R_W = 0x74 @@ -74,6 +76,11 @@ class MPU9250(object): SCALE_GYRO = 250, 500, 1000, 2000 SCALE_ACCEL = 2, 4, 8, 16 + # Set Rate + RATE_MAG_8HZ = 0x2 + RATE_MAG_100HZ = 0x6 + + REG_ID = 0x75 # WHOAMI REG MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG @@ -108,7 +115,7 @@ def __init__(self, spi, cs=0): self.writeRegister(self.REG_USER_CTRL, 0x32) # time.sleep(0.2) - # Init magnetometer + # Init magnetometer self.initMag() # Reset gyro and accel configs @@ -129,23 +136,41 @@ def __init__(self, spi, cs=0): # Done with init() + def initMag(self): - """ Initalize on-die AK8963 magnetometer & get offset""" + """ Initalize on-die AK8963 magnetometer & get offset + Defauts: + MODE1 = 8 Hz countinous polling + """ # Soft Reset self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL2) self.writeRegister(self.REG_I2C_SLV0_DO, 0x01) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - time.sleep(0.1) # Stabilize + time.sleep(0.2) # Stabilize # Set 16-bit output & continuous MODE1 + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + time.sleep(0.2) # Stabilize + + + # Check status + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + AKCTRL1 = self.readRegister(73, 1)[0] + #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] + #assert whoami_ak[0] == 0x48, "" + print '\n AK893_CNTL1 = {:#010b}'.format(AKCTRL1) + + print "\nMagnetometer initialization complete." - def ak8963Whoami(self): + def magWhoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) # READ Flag + 0x0C is AK slave addr @@ -155,6 +180,52 @@ def ak8963Whoami(self): print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] assert whoami_ak[0] == 0x48, "AK8963 not detected on internal I2C bus" + def setRateMag(self, rateVal): + """ Sets the polling rate for the AK8963 magnetometer + Possible Values; + rangeVal == self.RATE_MAG_\8HZ\100HZ (All caps!) + """ + # We only accept rateVal = 0x2 (0010 = 8 Hz) or 0x6 (0110 = 100Hz) + if (rateVal != 0x2 and rateVal != 0x6 ): + print "\n Invalid RATE_MAG value! Rate not changed" + # @TODO Error handling + return -1 + else: + # Preserve previous REG bits + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + regOld = self.readRegister(73, 1)[0] + + regOld &= ~(0xF) # Clear [0:4] MODE bits + # Combine regOld and rateVal to set mode + regVal = regOld | (rateVal) + + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_DO, regVal) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + + + self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + magConf = self.readRegister(73, 1)[0] + + # test if we did it right?? + if (regVal == magConf): + # Update Current range variable + self.currentRateMag = rateVal + return 0 + else: + print "\n WARNING! FAILED TO SET magnetometer rate!" + print "\n\tI've set AK8963_CNTL1 to %d, wanted to set to %d" % ( + magConf, regVal) + return -2 + # @TODO Add proper error log + + + def setRangeGyro(self, rangeVal): """ Sets the readout range for the gyroscope Possible values @@ -162,7 +233,7 @@ def setRangeGyro(self, rangeVal): """ # We only accept rangeVal = 0 to 3 if (rangeVal < 0 or rangeVal > 3): - print "\n Invalid RANGE value! Range not changed" + print "\n Invalid RANGE_GRYO value! Range not changed" # @TODO Error handling return -1 else: @@ -194,7 +265,7 @@ def setRangeAccel(self, rangeVal): """ # We only accept rangeVal = 0 to 3 if (rangeVal < 0 or rangeVal > 3): - print "\n Invalid RANGE value! Range not changed" + print "\n Invalid RANGE_ACCEL value! Range not changed" # @TODO Error handling return -1 else: @@ -641,3 +712,7 @@ def toSigned16(self, int): if int < 0: return self.toUnsigned16(65536 + int ) # int is negative return self.toUnsigned16(int) + + # def readRegisterSLV0(self, addr, n_bytes=1) + + diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 06d7126..0a531bd 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -21,8 +21,20 @@ def setup(): delay(200) # Let the I2C reset settle # Sanity check - mpu.ak8963Whoami() - + mpu.magWhoami() # Do we have a line to the AK8963? + + # Set mag rate to 100Hz (default = 8hz) + #mpu.setRateMag(mpu.RATE_MAG_100HZ); + + # check mag CNTL1 reg + mpu.writeRegister(mpu.REG_I2C_SLV0_ADDR, 0x8C) + mpu.writeRegister(mpu.REG_I2C_SLV0_REG, mpu.AK8963_CNTL1) + mpu.writeRegister(mpu.REG_I2C_SLV0_CTRL, 0x81) + AKCTRL1 = mpu.readRegister(73, 1)[0] + #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] + #assert whoami_ak[0] == 0x48, "" + print '\n AK893_CNTL1 = {:#010b}'.format(AKCTRL1) + # Change gyro range for fun mpu.setRangeGyro(mpu.RANGE_GYRO_500DPS) From 95516f018d1094e598f61ad54e2652d3c5f30ae5 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 6 Aug 2015 12:21:13 +0800 Subject: [PATCH 16/22] Added readRegisterSLV0() and writeRegisterSLV() helper methods to reduce code bloat when accessing magnetometer --- bbio/libraries/MPU9250/MPU9250.py | 39 ++++++++++++++++++++++++++++--- examples/MPU9250_test.py | 26 ++++++++++++++++----- 2 files changed, 56 insertions(+), 9 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index a98f02e..a96df1d 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -317,8 +317,9 @@ def getMag( self): self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) self.writeRegister(self.REG_I2C_SLV0_REG, 0x03) self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x87) # read 7 - msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 = self.readRegister(73,7) - + # We must read stat2, so that the mag puts out new data( it's a read flag) + msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 \ + = self.readRegister(self.REG_EXT_SENS_DATA_00, 7) scaling = self.currentRangeMag valX = self.fromSigned16([msbX, lsbX]) / 32768.0 * scaling valY = self.fromSigned16([msbY, lsbY]) / 32768.0 * scaling @@ -690,7 +691,40 @@ def readRegister(self, addr, n_bytes=1): def writeRegister(self, addr, value): """ Writes the given value to the given register. """ self.spi.write(self.cs, [addr & 0x7f, value & 0xff]) + + # Convenience function for communicating with the magnetometer via SPI + # or any SLV0 for that matter, just change hardcoded SLV0_ADDR + def readRegisterSLV0(self, addr, n_bytes=1): + """ Read n_bytes from addr address on SLV0, by putting data + in REG_EXT_SENS_DATA_00 and reading from there + returns read byte(s) + """ + SLV0_ADDR = 0x0C # That's for the AK8963 Magnetometer + SLV0_ADDR |= 0x80 # It's a read transfer + CTRL_HEX = 0x80 # We have to put bytes it on REG_EXT_SENS_DATA_00 + CTRL_HEX |= (n_bytes & 0xF) # How many bytes from addr? + # Maximum is 15 (way more than enough) + + self.writeRegister(self.REG_I2C_SLV0_ADDR, SLV0_ADDR) + self.writeRegister(self.REG_I2C_SLV0_REG, addr) # From which reg to read? + self.writeRegister(self.REG_I2C_SLV0_CTRL, CTRL_HEX) + # Wait a bit for the write + time.sleep(0.01) + return self.readRegister(self.REG_EXT_SENS_DATA_00, n_bytes) + + def writeRegisterSLV0(self, addr, value): + """ Writes the given value to the SLV0 register on address addr + SPI talks to the MPU9250, the MPU9250 writes via slave I2C + """ + SLV0_ADDR = 0x0C # That's for the AK8963 Magnetometer + SLV0_ADDR |= 0x00 # It's a write transfer + + self.writeRegister(self.REG_I2C_SLV0_ADDR, SLV0_ADDR) + self.writeRegister(self.REG_I2C_SLV0_REG, addr) + self.writeRegister(self.REG_I2C_SLV0_DO, value) + self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) # do the write + def fromUnsigned16(self, bytes): """ Convert register values as unsigned short int """ @@ -713,6 +747,5 @@ def toSigned16(self, int): return self.toUnsigned16(int) - # def readRegisterSLV0(self, addr, n_bytes=1) diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index 0a531bd..c050428 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -26,15 +26,29 @@ def setup(): # Set mag rate to 100Hz (default = 8hz) #mpu.setRateMag(mpu.RATE_MAG_100HZ); + delay(200) # check mag CNTL1 reg - mpu.writeRegister(mpu.REG_I2C_SLV0_ADDR, 0x8C) - mpu.writeRegister(mpu.REG_I2C_SLV0_REG, mpu.AK8963_CNTL1) - mpu.writeRegister(mpu.REG_I2C_SLV0_CTRL, 0x81) - AKCTRL1 = mpu.readRegister(73, 1)[0] - #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] - #assert whoami_ak[0] == 0x48, "" + #mpu.writeRegister(mpu.REG_I2C_SLV0_ADDR, 0x8C) + #mpu.writeRegister(mpu.REG_I2C_SLV0_REG, mpu.AK8963_CNTL1) + #mpu.writeRegister(mpu.REG_I2C_SLV0_CTRL, 0x81) + #AKCTRL1 = mpu.readRegister(73, 1)[0] + AKCTRL1 = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] print '\n AK893_CNTL1 = {:#010b}'.format(AKCTRL1) + # Read first 15 bytes from mag and print + datamag = mpu.readRegisterSLV0(0x00, 14) + print '\n\t %s' % datamag + + # Test read and write to mag + mpu.writeRegisterSLV0(mpu.AK8963_CNTL1, 0x02) #set 14 bit 8Hz continous + readReg = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] + print '\n AK893_CNTL1 changed to = {:#010b}'.format(readReg) + + # fix to 100Hz 16 bit + mpu.writeRegisterSLV0(mpu.AK8963_CNTL1, 0x16) #set 16 bit 100Hz cont + readReg = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] + print '\n AK893_CNTL1 changed to = {:#010b}'.format(readReg) + # Change gyro range for fun mpu.setRangeGyro(mpu.RANGE_GYRO_500DPS) From ffda9a357152078ae5c03197beab598f17c5e863 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Thu, 6 Aug 2015 12:35:13 +0800 Subject: [PATCH 17/22] used mag read methods to remove code bloat from setRateMag() --- bbio/libraries/MPU9250/MPU9250.py | 30 ++++++++++++++++++------------ examples/MPU9250_test.py | 8 ++------ 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index a96df1d..943852b 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -192,25 +192,31 @@ def setRateMag(self, rateVal): return -1 else: # Preserve previous REG bits - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - regOld = self.readRegister(73, 1)[0] + #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + #regOld = self.readRegister(73, 1)[0] + + regOld = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] regOld &= ~(0xF) # Clear [0:4] MODE bits # Combine regOld and rateVal to set mode regVal = regOld | (rateVal) - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_DO, regVal) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + self.writeRegisterSLV0(self.AK8963_CNTL1, regVal) + + #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) + #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + #self.writeRegister(self.REG_I2C_SLV0_DO, regVal) + #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - magConf = self.readRegister(73, 1)[0] + #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) + #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) + #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + #magConf = self.readRegister(73, 1)[0] + + magConf = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] # test if we did it right?? if (regVal == magConf): diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index c050428..e8f9717 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -24,16 +24,12 @@ def setup(): mpu.magWhoami() # Do we have a line to the AK8963? # Set mag rate to 100Hz (default = 8hz) - #mpu.setRateMag(mpu.RATE_MAG_100HZ); + mpu.setRateMag(mpu.RATE_MAG_100HZ); delay(200) # check mag CNTL1 reg - #mpu.writeRegister(mpu.REG_I2C_SLV0_ADDR, 0x8C) - #mpu.writeRegister(mpu.REG_I2C_SLV0_REG, mpu.AK8963_CNTL1) - #mpu.writeRegister(mpu.REG_I2C_SLV0_CTRL, 0x81) - #AKCTRL1 = mpu.readRegister(73, 1)[0] AKCTRL1 = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] - print '\n AK893_CNTL1 = {:#010b}'.format(AKCTRL1) + print '\n rate to 100Hz; AK893_CNTL1 = {:#010b}'.format(AKCTRL1) # Read first 15 bytes from mag and print datamag = mpu.readRegisterSLV0(0x00, 14) From 2aed1224e376895047a60aca50ca855e5a68033e Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 7 Aug 2015 02:24:56 +0800 Subject: [PATCH 18/22] Removed code bloat from magnetometer code --- bbio/libraries/MPU9250/MPU9250.py | 75 +++++++++++-------------------- 1 file changed, 25 insertions(+), 50 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 943852b..a636ad2 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -43,9 +43,6 @@ class MPU9250(object): REG_PWR_MGMT_1 = 0x6B # Device defaults to the SLEEP mode REG_PWR_MGMT_2 = 0x6C - AK8963_CNTL1 = 0x0A - AK8963_CNTL2 = 0x0B - REG_TEMP_OUT_H = 0x41 REG_TEMP_OUT_L = 0x42 @@ -80,6 +77,11 @@ class MPU9250(object): RATE_MAG_8HZ = 0x2 RATE_MAG_100HZ = 0x6 + # AK8963 magnetometer register addresses + AK8963_WHOAMI = 0x00 + AK8963_CNTL1 = 0x0A + AK8963_CNTL2 = 0x0B + REG_ID = 0x75 # WHOAMI REG MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG @@ -96,8 +98,8 @@ def __init__(self, spi, cs=0): # print "\nGot WHOAMI = 0x%02x" %id_val assert id_val == self.MPU9250_ID_VALUE, "MPU9250 not detected on SPI bus" - # @TODO Fix mysterious periodic hang of magnetometer ? - self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + # @TODO Fix mysterious periodic hang of magnetometer when full reset? + # self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers time.sleep(0.2) self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock source self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON @@ -143,42 +145,33 @@ def initMag(self): MODE1 = 8 Hz countinous polling """ # Soft Reset - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL2) - self.writeRegister(self.REG_I2C_SLV0_DO, 0x01) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + self.writeRegisterSLV0(self.AK8963_CNTL2, 0x01) time.sleep(0.2) # Stabilize # Set 16-bit output & continuous MODE1 - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_DO, 0x12 ) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x12) time.sleep(0.2) # Stabilize - - # Check status - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - AKCTRL1 = self.readRegister(73, 1)[0] - #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] - #assert whoami_ak[0] == 0x48, "" - print '\n AK893_CNTL1 = {:#010b}'.format(AKCTRL1) + AKCTNL1 = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] + #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak + #assert whoami_ak == 0x48, "" + print '\n AK893_CNTL1 = {:#010b}'.format(AKCTNL1) print "\nMagnetometer initialization complete." def magWhoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ - self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - # READ Flag + 0x0C is AK slave addr - self.writeRegister(self.REG_I2C_SLV0_REG, 0x00) - self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - whoami_ak = self.readRegister(73, 1) - print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak[0] - assert whoami_ak[0] == 0x48, "AK8963 not detected on internal I2C bus" +# self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) +# # READ Flag + 0x0C is AK slave addr +# self.writeRegister(self.REG_I2C_SLV0_REG, 0x00) +# self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) +# whoami_ak = self.readRegister(73, 1) + + whoami_ak = self.readRegisterSLV0(self.AK8963_WHOAMI, 1)[0] + print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak + assert whoami_ak == 0x48, "AK8963 not detected on internal I2C bus" def setRateMag(self, rateVal): """ Sets the polling rate for the AK8963 magnetometer @@ -192,32 +185,14 @@ def setRateMag(self, rateVal): return -1 else: # Preserve previous REG bits - #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - #regOld = self.readRegister(73, 1)[0] - regOld = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] - regOld &= ~(0xF) # Clear [0:4] MODE bits # Combine regOld and rateVal to set mode regVal = regOld | (rateVal) - + # Push new values self.writeRegisterSLV0(self.AK8963_CNTL1, regVal) - - #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x0C) - #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - #self.writeRegister(self.REG_I2C_SLV0_DO, regVal) - #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - - - #self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) - #self.writeRegister(self.REG_I2C_SLV0_REG, self.AK8963_CNTL1) - #self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) - #magConf = self.readRegister(73, 1)[0] - - magConf = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] - + + magConf = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] # test if we did it right?? if (regVal == magConf): # Update Current range variable From 3ebdac9b31d0986e6cd494a300d3e52a47bcab71 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Sun, 9 Aug 2015 15:32:47 +0800 Subject: [PATCH 19/22] Cleaned & commented; ready to go --- bbio/libraries/MPU9250/MPU9250.py | 79 ++++++++++++++++++++----------- examples/MPU9250_test.py | 56 ---------------------- 2 files changed, 52 insertions(+), 83 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index a636ad2..218d371 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -81,10 +81,10 @@ class MPU9250(object): AK8963_WHOAMI = 0x00 AK8963_CNTL1 = 0x0A AK8963_CNTL2 = 0x0B + AK8963_ASAX = 0x10 - - REG_ID = 0x75 # WHOAMI REG - MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG + REG_ID = 0x75 # WHOAMI REG + MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG def __init__(self, spi, cs=0): self.spi = spi @@ -98,22 +98,20 @@ def __init__(self, spi, cs=0): # print "\nGot WHOAMI = 0x%02x" %id_val assert id_val == self.MPU9250_ID_VALUE, "MPU9250 not detected on SPI bus" + # Power down mag + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x00) + time.sleep(0.1) + # @TODO Fix mysterious periodic hang of magnetometer when full reset? - # self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers + #self.writeRegister(self.REG_PWR_MGMT_1, 0x80) # Reset internal registers time.sleep(0.2) self.writeRegister(self.REG_PWR_MGMT_1, 0x01) # Auto select best clock source self.writeRegister(self.REG_PWR_MGMT_2, 0x00) # Gyro & Accel ON - time.sleep(0.2) + #time.sleep(0.2) - - accelOffsetData = self.readRegister( self.REG_XA_OFFSET_H, 2) - print '\n AccelX_H: {:#010b}'.format(accelOffsetData[0]) - print '\n AccelX_L: {:#010b}'.format(accelOffsetData[1]) - - - #This part is to set up the magnetometer, due to it being a separate device self.writeRegister(self.REG_I2C_MST_CTRL, 0x0D) # I2C speed 400 Khz + time.sleep(0.3) self.writeRegister(self.REG_USER_CTRL, 0x32) # time.sleep(0.2) @@ -144,6 +142,29 @@ def initMag(self): Defauts: MODE1 = 8 Hz countinous polling """ + # Power down mag + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x00) + time.sleep(0.1) + + # enter Fuse ROM access mode + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x0F) + time.sleep(0.1) + # read mag sensor factory sensitivity adjustments values + ASAX, ASAY, ASAZ = self.readRegisterSLV0(self.AK8963_ASAX, 3) + #print "X,Y,Z: %f %f %f " % (ASAX, ASAY, ASAZ) + + # Compute mag sensor adjust factors, formulas from AK8963 datasheet + self.magAdjustX = ((ASAX - 128) * 0.5) / 128 + 1 + self.magAdjustY = ((ASAY - 128) * 0.5) / 128 + 1 + self.magAdjustZ = ((ASAZ - 128) * 0.5) / 128 + 1 + + #print "Adjust X,Y,Z: %f %f %f " % (self.magAdjustX, self.magAdjustY, self.magAdjustZ) + + + # Power down mag + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x00) + time.sleep(0.1) + # Soft Reset self.writeRegisterSLV0(self.AK8963_CNTL2, 0x01) time.sleep(0.2) # Stabilize @@ -157,18 +178,11 @@ def initMag(self): #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak #assert whoami_ak == 0x48, "" print '\n AK893_CNTL1 = {:#010b}'.format(AKCTNL1) - print "\nMagnetometer initialization complete." def magWhoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ -# self.writeRegister(self.REG_I2C_SLV0_ADDR, 0x8C) -# # READ Flag + 0x0C is AK slave addr -# self.writeRegister(self.REG_I2C_SLV0_REG, 0x00) -# self.writeRegister(self.REG_I2C_SLV0_CTRL, 0x81) -# whoami_ak = self.readRegister(73, 1) - whoami_ak = self.readRegisterSLV0(self.AK8963_WHOAMI, 1)[0] print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak assert whoami_ak == 0x48, "AK8963 not detected on internal I2C bus" @@ -301,10 +315,11 @@ def getMag( self): # We must read stat2, so that the mag puts out new data( it's a read flag) msbX, lsbX, msbY, lsbY, msbZ, lsbZ, stat2 \ = self.readRegister(self.REG_EXT_SENS_DATA_00, 7) + # scaling for data reads and magnetometer adjustment factors (factory set) scaling = self.currentRangeMag - valX = self.fromSigned16([msbX, lsbX]) / 32768.0 * scaling - valY = self.fromSigned16([msbY, lsbY]) / 32768.0 * scaling - valZ = self.fromSigned16([msbZ, lsbZ]) / 32768.0 * scaling + valX = self.fromSigned16([msbX, lsbX])/ 32768.0 * scaling * self.magAdjustX + valY = self.fromSigned16([msbY, lsbY])/ 32768.0 * scaling * self.magAdjustY + valZ = self.fromSigned16([msbZ, lsbZ])/ 32768.0 * scaling * self.magAdjustZ return [valX, valY, valZ] def getTemp(self): @@ -335,6 +350,9 @@ def runSelfTest(self): STSumAccel = [0] * 200 STSumGyro = [0] * 200 + # Read ranges for accel and gyro + oldRangeAccel = self.currentRangeAccel + oldRangeGyro = self.currentRangeGyro # Read configs for restoration oldRegConf = self.readRegister(self.REG_CONFIG, 1)[0] @@ -433,6 +451,11 @@ def runSelfTest(self): self.writeRegister(self.REG_ACCEL_CONFIG1, oldAccelConf1) self.writeRegister(self.REG_ACCEL_CONFIG2, oldAccelConf2) + # Restore ranges properly + self.currentRangeAccel = oldRangeAccel + self.currentRangeGyro = oldRangeGyro + + return devAccel, devGyro @@ -445,6 +468,9 @@ def calibrateAccelGyro(self): biasAccel = [0] * 3 out = [0] * 6 + # Read ranges for accel and gyro + oldRangeAccel = self.currentRangeAccel + oldRangeGyro = self.currentRangeGyro # Read configs for restoration oldSampleDiv = self.readRegister(self.REG_SMPLRT_DIV, 1)[0] @@ -472,8 +498,6 @@ def calibrateAccelGyro(self): self.writeRegister(self.REG_CONFIG, 0x01) # Set low-pass filter to 188 Hz self.writeRegister(self.REG_SMPLRT_DIV, 0x00) # Set sample rate to 1 kHz - # self.setRangeGyro(self.RANGE_GYRO_250DPS) # Set gyro full-scale to 250 degrees per second, maximum sensitivity - # self.setRangeAccel(self.RANGE_ACCEL_2G) # Set accelerometer full-scale to 2 g, maximum sensitivity self.writeRegister(self.REG_GYRO_CONFIG, 0x00) # Set gyro full-scale to 250 degrees per second, maximum sensitivity self.writeRegister(self.REG_ACCEL_CONFIG1, 0x00) # Set accelerometer full-scale to 2 g, maximum sensitivity @@ -493,7 +517,6 @@ def calibrateAccelGyro(self): fifoCount = fifoData[0]<<8 | fifoData[1] packetCount = fifoCount/12 # How many sets of full gyro and accelerometer data for averaging - # print " EXTRAS: %d" % (fifoCount % 12) # ALIGN FIFO: Read extra bits in buffer (and remove them)!!! for j in range(fifoCount % 12): self.readRegister(self.REG_FIFO_R_W, 1)[0] @@ -655,8 +678,10 @@ def calibrateAccelGyro(self): self.writeRegister(self.REG_ACCEL_CONFIG1, oldAccelConf1) self.writeRegister(self.REG_ACCEL_CONFIG2, oldAccelConf2) - # Set scale properly - + # Restore ranges properly + self.currentRangeAccel = oldRangeAccel + self.currentRangeGyro = oldRangeGyro + return out diff --git a/examples/MPU9250_test.py b/examples/MPU9250_test.py index e8f9717..6045818 100644 --- a/examples/MPU9250_test.py +++ b/examples/MPU9250_test.py @@ -26,43 +26,6 @@ def setup(): # Set mag rate to 100Hz (default = 8hz) mpu.setRateMag(mpu.RATE_MAG_100HZ); - delay(200) - # check mag CNTL1 reg - AKCTRL1 = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] - print '\n rate to 100Hz; AK893_CNTL1 = {:#010b}'.format(AKCTRL1) - - # Read first 15 bytes from mag and print - datamag = mpu.readRegisterSLV0(0x00, 14) - print '\n\t %s' % datamag - - # Test read and write to mag - mpu.writeRegisterSLV0(mpu.AK8963_CNTL1, 0x02) #set 14 bit 8Hz continous - readReg = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] - print '\n AK893_CNTL1 changed to = {:#010b}'.format(readReg) - - # fix to 100Hz 16 bit - mpu.writeRegisterSLV0(mpu.AK8963_CNTL1, 0x16) #set 16 bit 100Hz cont - readReg = mpu.readRegisterSLV0(mpu.AK8963_CNTL1, 1)[0] - print '\n AK893_CNTL1 changed to = {:#010b}'.format(readReg) - - # Change gyro range for fun - mpu.setRangeGyro(mpu.RANGE_GYRO_500DPS) - - delay(500) - confData = mpu.readRegister( 27, 2) - print '\n GyroConfig: {:#010b}'.format(confData[0]) - print '\n AccelConfig: {:#010b}'.format(confData[1]) - - # Change it back to maximum - mpu.setRangeGyro(mpu.RANGE_GYRO_2000DPS) - # Can we mess it up? - mpu.setRangeGyro(27) - - delay(500) - confData = mpu.readRegister( 27, 2) - print '\n GyroConfig: {:#010b}'.format(confData[0]) - print '\n AccelConfig: {:#010b}'.format(confData[1]) - # Do a selftest before we start devAccel, devGyro = mpu.runSelfTest() @@ -76,25 +39,6 @@ def setup(): print "\n Sensor offset (bias) values:" print "\n\t ACCEL BIAS: %f %f %f\n\n\t GYRO BIAS: %f %f %f " % (data[0],data[1],data[2],data[3],data[4],data[5]) - - # Did we fix everything when we're out of the calibration - delay(500) - confData = mpu.readRegister( 27, 2) - print '\n AccelConfig: {:#010b}'.format(confData[1]) - print '\n GyroConfig: {:#010b}'.format(confData[0]) - delay(500) - - - accelOffsetData = mpu.readRegister( 119, 2) - print '\n AccelX_H: {:#010b}'.format(accelOffsetData[0]) - print '\n AccelX_L: {:#010b}'.format(accelOffsetData[1]) - - scale = mpu.currentRangeAccel - print "\n ACCEL RANGE = %d" % mpu.SCALE_ACCEL[scale] - - mpu.currentRangeAccel = 3 - mpu.currentRangeGyro = 3 - def loop(): # Get data From 507126d6fa11bd550ad6f3a1a9c64a248d2b099a Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Mon, 10 Aug 2015 16:25:07 +0800 Subject: [PATCH 20/22] minor edit for BeagleSat testing --- bbio/libraries/MPU9250/MPU9250.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 218d371..84362bb 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -85,18 +85,24 @@ class MPU9250(object): REG_ID = 0x75 # WHOAMI REG MPU9250_ID_VALUE = 0x71 # precoded identification string in WHOAMI REG + def __init__(self, spi, cs=0): + + self.sensorOnline = 0 + self.spi = spi self.cs = cs spi.begin() spi.setClockMode(0, 0) spi.setMaxFrequency(0, 1000000) + # Am I talking to an MPU9250? id_val = self.readRegister(self.REG_ID)[0] # print "\nGot WHOAMI = 0x%02x" %id_val assert id_val == self.MPU9250_ID_VALUE, "MPU9250 not detected on SPI bus" + # Power down mag self.writeRegisterSLV0(self.AK8963_CNTL1, 0x00) @@ -134,8 +140,9 @@ def __init__(self, spi, cs=0): #self.CurrentRangeGyro = self.RANGE_GYRO_2000DPS #self.CurrentRangeAccel = self.RANGE_ACCEL_16G + self.sensorOnline = 1 # If we passed all setup procedures, we're good # Done with init() - + def initMag(self): """ Initalize on-die AK8963 magnetometer & get offset @@ -159,7 +166,6 @@ def initMag(self): self.magAdjustZ = ((ASAZ - 128) * 0.5) / 128 + 1 #print "Adjust X,Y,Z: %f %f %f " % (self.magAdjustX, self.magAdjustY, self.magAdjustZ) - # Power down mag self.writeRegisterSLV0(self.AK8963_CNTL1, 0x00) From 958eda16b10d33a31f6ae4c71a5c9304d69b00e8 Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 21 Aug 2015 13:10:34 +0200 Subject: [PATCH 21/22] cleaned/removed init messages from initMag() --- bbio/libraries/MPU9250/MPU9250.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 84362bb..261eb43 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -183,9 +183,9 @@ def initMag(self): AKCTNL1 = self.readRegisterSLV0(self.AK8963_CNTL1, 1)[0] #print "\nGot WHOAMI for AK8963 = 0x%02x (0x48?) " % whoami_ak #assert whoami_ak == 0x48, "" - print '\n AK893_CNTL1 = {:#010b}'.format(AKCTNL1) + #print '\n AK893_CNTL1 = {:#010b}'.format(AKCTNL1) - print "\nMagnetometer initialization complete." + print "\nMagnetometer initialization complete.\n" def magWhoami(self): """ I2C WhoAmI check for the AK8963 in-built magnetometer """ From 43460ddd950d2a3e3271094d6687f1dcd637bb1d Mon Sep 17 00:00:00 2001 From: Niko Visnjic Date: Fri, 21 Aug 2015 16:20:31 +0200 Subject: [PATCH 22/22] changed default magnetometer polling rate to 100 Hz --- bbio/libraries/MPU9250/MPU9250.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bbio/libraries/MPU9250/MPU9250.py b/bbio/libraries/MPU9250/MPU9250.py index 261eb43..6b70bd8 100755 --- a/bbio/libraries/MPU9250/MPU9250.py +++ b/bbio/libraries/MPU9250/MPU9250.py @@ -175,8 +175,8 @@ def initMag(self): self.writeRegisterSLV0(self.AK8963_CNTL2, 0x01) time.sleep(0.2) # Stabilize - # Set 16-bit output & continuous MODE1 - self.writeRegisterSLV0(self.AK8963_CNTL1, 0x12) + # Set 16-bit output & continuous MODE2 (100 Hz + self.writeRegisterSLV0(self.AK8963_CNTL1, 0x16) time.sleep(0.2) # Stabilize # Check status