MPU-9250 Teensy Library

Hi Brian,

That actually seems like the better way to go this way it makes it easier for configuration control. Right now I am in the middle of moding a Onagofly so I will test some time this week. I have a couple of Arduino boards including a Due and a Curie. I will test in the next day or so.

By the way I did find the reference for the Extended command filter, https://github.com/matthew-t-watson/Picopter. I am not sure his thesis is still on-line but if you want it I have a copy, http://www.recantha.co.uk/blog/?p=6346

Mike
 
Hi Brian

I completed mods to the library to make it work with non-Teensy boards. Had to do a few more changes than what you did to account for having to change from t2 to wire (supports 2 wire ports, easy enough to change for more). I test the library with an ESP32, Arduino Mega 2560 and the Arduino 101 (Curie) boards using i2c and no problems. Have to find the Due then I would think it would work with an i2c using standard wire.

I have to complete the coding for SPI but unfortunately I don't have a 9250 that supports the spi bus at the moment. Just ordered one. Should be here next week. Never really used spi so this should be interesting.

Mike
 
Hi Brian

I completed mods to the library to make it work with non-Teensy boards. Had to do a few more changes than what you did to account for having to change from t2 to wire (supports 2 wire ports, easy enough to change for more). I test the library with an ESP32, Arduino Mega 2560 and the Arduino 101 (Curie) boards using i2c and no problems. Have to find the Due then I would think it would work with an i2c using standard wire.

I have to complete the coding for SPI but unfortunately I don't have a 9250 that supports the spi bus at the moment. Just ordered one. Should be here next week. Never really used spi so this should be interesting.

Mike

Cool, great to hear! If you want to submit pull requests for the mods, I'll incorporate them.

Brian
 
Let me work on getting SPI working first then I will submit the pull request.

Mike

UPDATE: Pull request completed.
 
Last edited:
Hi Brian,

Been doing some testing with the 9250 and I am noticing something interesting. At times if I am moving the 9250 rapidly some of the component values go zero. When I steady the board the values ready to normal. Here is a quick example

Code:
3600	-296	2052	-19209	-6237	-32767	69	-117	-379	3488
-1970	-142	3752	-21077	-3735	-32767	139	-93	-397	3408
0	-16842	0	-21077	-3735	-26135	8191	5976	8191	97
0	-5578	0	-21077	-3735	-26135	8191	5976	8191	65
0	-16842	0	-21077	-3735	-26135	8191	5976	8191	97
0	-4554	0	-21077	-3735	-26135	8191	5976	8191	65
-5292	1290	5090	1444	4741	-6087	222	42	-392	3488
-5572	376	5172	1539	6229	-12201	227	39	-389	3376

The extent varies. This is a short version. Sometimes its for about 20 values. This happens with your lib as well as mine ported version to the FreeIMU lib.

It this a data overflow or data ready issue. Should be testing for this?

Mike
 
Hi Brian,

Been doing some testing with the 9250 and I am noticing something interesting. At times if I am moving the 9250 rapidly some of the component values go zero. When I steady the board the values ready to normal. Here is a quick example

Code:
3600	-296	2052	-19209	-6237	-32767	69	-117	-379	3488
-1970	-142	3752	-21077	-3735	-32767	139	-93	-397	3408
0	-16842	0	-21077	-3735	-26135	8191	5976	8191	97
0	-5578	0	-21077	-3735	-26135	8191	5976	8191	65
0	-16842	0	-21077	-3735	-26135	8191	5976	8191	97
0	-4554	0	-21077	-3735	-26135	8191	5976	8191	65
-5292	1290	5090	1444	4741	-6087	222	42	-392	3488
-5572	376	5172	1539	6229	-12201	227	39	-389	3376

The extent varies. This is a short version. Sometimes its for about 20 values. This happens with your lib as well as mine ported version to the FreeIMU lib.

It this a data overflow or data ready issue. Should be testing for this?

Mike

Thanks Mike, which function was called to get this data and how often were you calling it? I assume no filtering was used...

Brian
 
Hi Brian

Used getMotion10Counts and then tried it with individual calls to getAccel, getGyro and GetMag. Running a teensy 3.5 at 120Mhz and tried a varied amount of delays (50 and 100). Ran at 0.002ms, 0.03 and 0.1 but got pretty much same results. Interesting is that it like pauses when the problem happens. In my version it is quite noticeable. No filtering. Think I need to add because when I look at the values closely their is variation more than what I thought.

Thanks
Mike
 
That's really cool! I like the visualization. What's the second IMU for? Or are you going to be tracking arm motion per the UMass link?

Thanks for sharing.
Brian
 
I am going to start with the implementation from the UMass link but I started looking at the hand and arm. Probably will need three IMUs and flexsensors for the fingers. Looking at using unity for the interface. Have to finish off a couple of other projects first.

Mike
 
Hi

I'm not having much luck with getting your library working. I managed to get various other libraries working, such as the Sparkfun one and the in-sketch method from Kris Winer, but this library eludes me.

It always returns a beginStatus of -1. I have looked into the files and have tried a few different settings such as defining the pins and not defining them, using internal and external pullups(4.7k), although after I read the Teensy 3.6 that I'm using doesn't have internal pullups.

Either way, I have it working fine with other libraries. I have also been having lots of problems similar to that described by ninja2 of it hanging after reprogramming, which can normally be alleviated by a power cycle or two. I also have the problem of the data stream being sent to the Arduino IDE serial monitor freezing, but haven't looked into this to determine whether the MPU9250 is the source of this or not.

Do you have any suggestions about test I could try to determine why the library isn't working?

As mentioned, I'm using a Teensy3.6 and have a cheap Ebay breakout that is marked as MPU-92/65. I have SDA connected to 18 and SCL to 19 with 4.7k pullups on both. I also currently have the interrupt pin connected to 2 on the Teensy and the AD0 is connected to ground.

Thanks
 
Hi

I'm not having much luck with getting your library working. I managed to get various other libraries working, such as the Sparkfun one and the in-sketch method from Kris Winer, but this library eludes me.

It always returns a beginStatus of -1. I have looked into the files and have tried a few different settings such as defining the pins and not defining them, using internal and external pullups(4.7k), although after I read the Teensy 3.6 that I'm using doesn't have internal pullups.

Either way, I have it working fine with other libraries. I have also been having lots of problems similar to that described by ninja2 of it hanging after reprogramming, which can normally be alleviated by a power cycle or two. I also have the problem of the data stream being sent to the Arduino IDE serial monitor freezing, but haven't looked into this to determine whether the MPU9250 is the source of this or not.

Do you have any suggestions about test I could try to determine why the library isn't working?

As mentioned, I'm using a Teensy3.6 and have a cheap Ebay breakout that is marked as MPU-92/65. I have SDA connected to 18 and SCL to 19 with 4.7k pullups on both. I also currently have the interrupt pin connected to 2 on the Teensy and the AD0 is connected to ground.

Thanks

From a quick google search, it looks like you're using an MPU-9255, not the MPU-9250. That being said, a quick look through the register map looks similar enough that the libraries should be compatible.

Can you post a photo of your wiring? I thought I had all the initialization issues worked out, so it's interesting that yours isn't working. In the begin function, you can change the return values to be different values (i.e. -1, -2, -3, etc) to nail down exactly which register it's getting tripped up on. My guess is that it would be one of the magnetometer (AK8963) register settings.

Brian
 
Brian

Who am value changes for the MPU9255. For the 9255 it has to be 115, for the 9250 its 113 as you had it. I just changed it to 115 in your library and it works fine. Its line 303 in the MPU9250.cpp. Found out because I tried the mods in my lib, I printed out a bunch more info and it failed at the who am I test, but everything else passed and the data was good. Just realized that I a 9255 laying around.

Think this is what it should be changed to:

Code:
    // check the WHO AM I byte, expected value is 0x71 (decimal 113)
    if( whoAmI() == 113 || whoAmI() == 115){
        return -1;
    }
 
Last edited:
Brilliant, thanks both for your help. It's working great now.

Brian, I should have done more research into this device. Some info on the web says it to be the 9250 and some say it's the 9255. Hope this helps anyone else.

mjs513, thanks very much for the tip. The whoAmI of 115 works perfectly. Although your code piece returns -1 if the register is correct. I changed the conditional statement to:
if( whoAmI() != 113 && whoAmI() != 115 ){

Thanks a lot
 
Hi

I'm working on trying to use Kris Winer's Madgwick filter from the MPU9250 sample on his Github and combining it into this library so I can have a stable and easy to use quaternion value to calculate the direction of gravity. I'm working towards having all calculations for getting a velocity value housed within the library.

I'm having trouble with calibrating the raw values and can't seem to find where the problem is. I think it might be related to the transformation matrix but I'm not sure. I don't really understand this matrix. Why does it always flip the X and Y values on Gyro and Accel?

In Kris Winer's Madgwick function, the Mag axes are flipped but only when the function is called.

As I understand it, I have copied the calibration function across from Kris Winer's example and it does not implement transformation. I have called this after calling the .begin initialisation of this library, so to avoid the soft reset wiping the biases. Then when I get the latest values using the original functions of this library, am I right in thinking that the biases are being applied to the X and Y axis in reverse?

Another question is, is there any other way to alter the values being read other than changing the values stored in the bias registers? Like, would initialising the wrong things or in the wrong order influence the output at all?

Sorry if this question is a little muddled. I've copied the altered library below and how I call it with getAccelCounts

Thanks

Code:
  IMU.getAccelCounts(&iax, &iay, &iaz);
  float aRes = 2.0/32768.0;
  aqx = float(iax) * aRes;
  aqy = float(iay) * aRes;
  aqz = float(iaz) * aRes;

Code:
/*
MPU9250.cpp
Brian R Taylor
[email]brian.taylor@bolderflight.com[/email]
2017-01-04

Copyright (c) 2016 Bolder Flight Systems

Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
and associated documentation files (the "Software"), to deal in the Software without restriction, 
including without limitation the rights to use, copy, modify, merge, publish, distribute, 
sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or 
substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING 
BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 
NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 
DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

// Teensy 3.0 || Teensy 3.1/3.2 || Teensy 3.5 || Teensy 3.6 || Teensy LC 
#if defined(__MK20DX128__) || defined(__MK20DX256__) || defined(__MK64FX512__) || \
	defined(__MK66FX1M0__) || defined(__MKL26Z64__)

#include "Arduino.h"
#include "MPU9250.h"
#include "i2c_t3.h"  // I2C library
#include "SPI.h" // SPI Library

/* MPU9250 object, input the I2C address and I2C bus */
MPU9250::MPU9250(uint8_t address, uint8_t bus){
    _address = address; // I2C address
    _bus = bus; // I2C bus
    _userDefI2C = false; // automatic I2C setup
    _useSPI = false; // set to use I2C instead of SPI
}

/* MPU9250 object, input the I2C address, I2C bus, and I2C pins */
MPU9250::MPU9250(uint8_t address, uint8_t bus, i2c_pins pins){
    _address = address; // I2C address
    _bus = bus; // I2C bus
    _pins = pins; // I2C pins
    _pullups = I2C_PULLUP_EXT; // I2C pullups
    _userDefI2C = true; // user defined I2C
    _useSPI = false; // set to use I2C instead of SPI
}

/* MPU9250 object, input the I2C address, I2C bus, I2C pins, and I2C pullups */
MPU9250::MPU9250(uint8_t address, uint8_t bus, i2c_pins pins, i2c_pullup pullups){
    _address = address; // I2C address
    _bus = bus; // I2C bus
    _pins = pins; // I2C pins
    _pullups = pullups; // I2C pullups
    _userDefI2C = true; // user defined I2C
    _useSPI = false; // set to use I2C instead of SPI
}

/* MPU9250 object, input the SPI CS Pin */
MPU9250::MPU9250(uint8_t csPin){
    _csPin = csPin; // SPI CS Pin
    _mosiPin = MOSI_PIN_11;	// SPI MOSI Pin, set to default
    _useSPI = true; // set to use SPI instead of I2C
    _useSPIHS = false; // defaul to low speed SPI transactions until data reads start to occur
}

/* MPU9250 object, input the SPI CS Pin and MOSI Pin */
MPU9250::MPU9250(uint8_t csPin, spi_mosi_pin pin){
    _csPin = csPin; // SPI CS Pin
    _mosiPin = pin;	// SPI MOSI Pin
    _useSPI = true; // set to use SPI instead of I2C
    _useSPIHS = false; // defaul to low speed SPI transactions until data reads start to occur
}

/* starts I2C communication and sets up the MPU-9250 */
int MPU9250::begin(mpu9250_accel_range accelRange, mpu9250_gyro_range gyroRange){
    uint8_t buff[3];
    uint8_t data[7];

    if( _useSPI ){ // using SPI for communication

        // setting CS pin to output
        pinMode(_csPin,OUTPUT);

        // setting CS pin high
        digitalWriteFast(_csPin,HIGH);

        // Teensy 3.0 || Teensy 3.1/3.2
		#if defined(__MK20DX128__) || defined(__MK20DX256__)

	        // configure and begin the SPI
	        switch( _mosiPin ){

				case MOSI_PIN_7:	// SPI bus 0 alternate 1
				    SPI.setMOSI(7);
	        		SPI.setMISO(8);
	        		SPI.setSCK(14);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_11:	// SPI bus 0 default
					SPI.setMOSI(11);
	        		SPI.setMISO(12);
	        		SPI.setSCK(13);
	        		SPI.begin();
	        		break;
	        }

        #endif

        // Teensy 3.5 || Teensy 3.6 
		#if defined(__MK64FX512__) || defined(__MK66FX1M0__)

	        // configure and begin the SPI
	        switch( _mosiPin ){

	        	case MOSI_PIN_0:	// SPI bus 1 default
	        		SPI1.setMOSI(0);
	        		SPI1.setMISO(1);
	        		SPI1.setSCK(32);
	        		SPI1.begin();
	        		break;
				case MOSI_PIN_7:	// SPI bus 0 alternate 1
				    SPI.setMOSI(7);
	        		SPI.setMISO(8);
	        		SPI.setSCK(14);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_11:	// SPI bus 0 default
					SPI.setMOSI(11);
	        		SPI.setMISO(12);
	        		SPI.setSCK(13);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_21:	// SPI bus 1 alternate
		        	SPI1.setMOSI(21);
	        		SPI1.setMISO(5);
	        		SPI1.setSCK(20);
	        		SPI1.begin();
	        		break;
				case MOSI_PIN_28:	// SPI bus 0 alternate 2
	        		SPI.setMOSI(28);
	        		SPI.setMISO(39);
	        		SPI.setSCK(27);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_44:	// SPI bus 2 default
	        		SPI2.setMOSI(44);
	        		SPI2.setMISO(45);
	        		SPI2.setSCK(46);
	        		SPI2.begin();
	        		break;
				case MOSI_PIN_52:	// SPI bus 2 alternate
	        		SPI2.setMOSI(52);
	        		SPI2.setMISO(51);
	        		SPI2.setSCK(53);
	        		SPI2.begin();
	        		break;
	        }

        #endif

        // Teensy LC 
		#if defined(__MKL26Z64__)

			// configure and begin the SPI
	        switch( _mosiPin ){

	        	case MOSI_PIN_0:	// SPI bus 1 default
	        		SPI1.setMOSI(0);
	        		SPI1.setMISO(1);
	        		SPI1.setSCK(20);
	        		SPI1.begin();
	        		break;
				case MOSI_PIN_7:	// SPI bus 0 alternate 1
				    SPI.setMOSI(7);
	        		SPI.setMISO(8);
	        		SPI.setSCK(14);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_11:	// SPI bus 0 default
					SPI.setMOSI(11);
	        		SPI.setMISO(12);
	        		SPI.setSCK(13);
	        		SPI.begin();
	        		break;
				case MOSI_PIN_21:	// SPI bus 1 alternate
		        	SPI1.setMOSI(21);
	        		SPI1.setMISO(5);
	        		SPI1.setSCK(20);
	        		SPI1.begin();
	        		break;
	        }

		#endif
    }
    else{ // using I2C for communication

        if( !_userDefI2C ) { // setup the I2C pins and pullups based on bus number if not defined by user
            /* setting the I2C pins, pullups, and protecting against _bus out of range */
            _pullups = I2C_PULLUP_EXT; // default to external pullups

            #if defined(__MK20DX128__) // Teensy 3.0
                _pins = I2C_PINS_18_19;
                _bus = 0;
            #endif

            #if defined(__MK20DX256__) // Teensy 3.1/3.2
                if(_bus == 1) {
                    _pins = I2C_PINS_29_30;
                }
                else{
                    _pins = I2C_PINS_18_19;
                    _bus = 0;
                }

            #endif

            #if defined(__MK64FX512__) // Teensy 3.5
                if(_bus == 2) {
                    _pins = I2C_PINS_3_4;
                }
                else if(_bus == 1) {
                    _pins = I2C_PINS_37_38;
                }
                else{
                    _pins = I2C_PINS_18_19;
                    _bus = 0;
                }

            #endif

            #if defined(__MK66FX1M0__) // Teensy 3.6
                if(_bus == 3) {
                    _pins = I2C_PINS_56_57;
                }
                else if(_bus == 2) {
                    _pins = I2C_PINS_3_4;
                }
                else if(_bus == 1) {
                    _pins = I2C_PINS_37_38;
                }
                else{
                    _pins = I2C_PINS_18_19;
                    _bus = 0;
                }

            #endif

            #if defined(__MKL26Z64__) // Teensy LC
                if(_bus == 1) {
                    _pins = I2C_PINS_22_23;
                }
                else{
                    _pins = I2C_PINS_18_19;
                    _bus = 0;
                }

            #endif
        }

        // starting the I2C bus
        i2c_t3(_bus).begin(I2C_MASTER, 0, _pins, _pullups, _i2cRate);
    }

    // select clock source to gyro
    if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
        return -1;
    }

    // enable I2C master mode
    if( !writeRegister(USER_CTRL,I2C_MST_EN) ){
        return -1;
    }

    // set the I2C bus speed to 400 kHz
    if( !writeRegister(I2C_MST_CTRL,I2C_MST_CLK) ){
        return -1;
    }

    // set AK8963 to Power Down
    if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
        return -1;
    }

    // reset the MPU9250
    writeRegister(PWR_MGMNT_1,PWR_RESET);

    // wait for MPU-9250 to come back up
    delay(1);

    // reset the AK8963
    writeAK8963Register(AK8963_CNTL2,AK8963_RESET);

    // select clock source to gyro
    if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
        return -1;
    }

    // check the WHO AM I byte, expected value is 0x71 (decimal 113)
    //MPU9250 whoAmI is 113. MPU9255 whoAmI is 115
    if( whoAmI() != 113 && whoAmI() != 115 ){
//    if( whoAmI() != 115 ){
//        Serial.print("whoami:");
//        Serial.print(whoAmI());
        return -1;
    }

    // enable accelerometer and gyro
    if( !writeRegister(PWR_MGMNT_2,SEN_ENABLE) ){
        return -1;
    }

    /* setup the accel and gyro ranges */
    switch(accelRange) {

        case ACCEL_RANGE_2G:
            // setting the accel range to 2G
            if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_2G) ){
                return -1;
            }
            _accelScale = G * 2.0f/32767.5f; // setting the accel scale to 2G
            break;

        case ACCEL_RANGE_4G:
            // setting the accel range to 4G
            if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_4G) ){
                return -1;
            }
            _accelScale = G * 4.0f/32767.5f; // setting the accel scale to 4G
            break;

        case ACCEL_RANGE_8G:
            // setting the accel range to 8G
            if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_8G) ){
                return -1;
            }
            _accelScale = G * 8.0f/32767.5f; // setting the accel scale to 8G
            break;

        case ACCEL_RANGE_16G:
            // setting the accel range to 16G
            if( !writeRegister(ACCEL_CONFIG,ACCEL_FS_SEL_16G) ){
                return -1;
            }
            _accelScale = G * 16.0f/32767.5f; // setting the accel scale to 16G
            break;
    }

    switch(gyroRange) {
        case GYRO_RANGE_250DPS:
            // setting the gyro range to 250DPS
            if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_250DPS) ){
                return -1;
            }
            _gyroScale = 250.0f/32767.5f * _d2r; // setting the gyro scale to 250DPS
            break;

        case GYRO_RANGE_500DPS:
            // setting the gyro range to 500DPS
            if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_500DPS) ){
                return -1;
            }
            _gyroScale = 500.0f/32767.5f * _d2r; // setting the gyro scale to 500DPS
            break;

        case GYRO_RANGE_1000DPS:
            // setting the gyro range to 1000DPS
            if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_1000DPS) ){
                return -1;
            }
            _gyroScale = 1000.0f/32767.5f * _d2r; // setting the gyro scale to 1000DPS
            break;

        case GYRO_RANGE_2000DPS:
            // setting the gyro range to 2000DPS
            if( !writeRegister(GYRO_CONFIG,GYRO_FS_SEL_2000DPS) ){
                return -1;
            }
            _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
            break;
    }

    // enable I2C master mode
    if( !writeRegister(USER_CTRL,I2C_MST_EN) ){
    	return -1;
    }

	// set the I2C bus speed to 400 kHz
	if( !writeRegister(I2C_MST_CTRL,I2C_MST_CLK) ){
		return -1;
	}

	// check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
	if( whoAmIAK8963() != 72 ){
        return -1;
	}

    /* get the magnetometer calibration */

    // set AK8963 to Power Down
    if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
        return -1;
    }
    delay(100); // long wait between AK8963 mode changes

    // set AK8963 to FUSE ROM access
    if( !writeAK8963Register(AK8963_CNTL1,AK8963_FUSE_ROM) ){
        return -1;
    }
    delay(100); // long wait between AK8963 mode changes

    // read the AK8963 ASA registers and compute magnetometer scale factors
    readAK8963Registers(AK8963_ASA,sizeof(buff),&buff[0]);
    _magScaleX = ((((float)buff[0]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
    _magScaleY = ((((float)buff[1]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla
    _magScaleZ = ((((float)buff[2]) - 128.0f)/(256.0f) + 1.0f) * 4912.0f / 32760.0f; // micro Tesla 

    // set AK8963 to Power Down
    if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
        return -1;
    }
    delay(100); // long wait between AK8963 mode changes  

    // set AK8963 to 16 bit resolution, 100 Hz update rate
    if( !writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS2) ){
        return -1;
    }
    delay(100); // long wait between AK8963 mode changes

    // select clock source to gyro
    if( !writeRegister(PWR_MGMNT_1,CLOCK_SEL_PLL) ){
        return -1;
    }       

    // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
    readAK8963Registers(AK8963_HXL,sizeof(data),&data[0]);

    // successful init, return 0
    return 0;
}


/* sets the DLPF and interrupt settings */
int MPU9250::setFilt(mpu9250_dlpf_bandwidth bandwidth, uint8_t SRD){
    uint8_t data[7];

    switch(bandwidth) {
        case DLPF_BANDWIDTH_184HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_184) ){ // setting accel bandwidth to 184Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_184) ){ // setting gyro bandwidth to 184Hz
                return -1;
            }
            break;

        case DLPF_BANDWIDTH_92HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_92) ){ // setting accel bandwidth to 92Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_92) ){ // setting gyro bandwidth to 92Hz
                return -1;
            }
            break; 

        case DLPF_BANDWIDTH_41HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_41) ){ // setting accel bandwidth to 41Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_41) ){ // setting gyro bandwidth to 41Hz
                return -1;
            } 
            break;

        case DLPF_BANDWIDTH_20HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_20) ){ // setting accel bandwidth to 20Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_20) ){ // setting gyro bandwidth to 20Hz
                return -1;
            }
            break;

        case DLPF_BANDWIDTH_10HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_10) ){ // setting accel bandwidth to 10Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_10) ){ // setting gyro bandwidth to 10Hz
                return -1;
            }
            break;

        case DLPF_BANDWIDTH_5HZ:
            if( !writeRegister(ACCEL_CONFIG2,ACCEL_DLPF_5) ){ // setting accel bandwidth to 5Hz
                return -1;
            } 
            if( !writeRegister(CONFIG,GYRO_DLPF_5) ){ // setting gyro bandwidth to 5Hz
                return -1;
            }
            break; 
    }

    /* setting the sample rate divider */
    if( !writeRegister(SMPDIV,SRD) ){ // setting the sample rate divider
        return -1;
    } 

    if(SRD > 9){

        // set AK8963 to Power Down
        if( !writeAK8963Register(AK8963_CNTL1,AK8963_PWR_DOWN) ){
            return -1;
        }
        delay(100); // long wait between AK8963 mode changes  

        // set AK8963 to 16 bit resolution, 8 Hz update rate
        if( !writeAK8963Register(AK8963_CNTL1,AK8963_CNT_MEAS1) ){
            return -1;
        }
        delay(100); // long wait between AK8963 mode changes     

        // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
        readAK8963Registers(AK8963_HXL,sizeof(data),&data[0]);
    }

    /* setting the interrupt */
    if( !writeRegister(INT_PIN_CFG,INT_PULSE_50US) ){ // setup interrupt, 50 us pulse
        return -1;
    }  
    if( !writeRegister(INT_ENABLE,INT_RAW_RDY_EN) ){ // set to data ready
        return -1;
    }  

    // successful filter setup, return 0
    return 0; 
}

/* enables and disables the interrupt */
int MPU9250::enableInt(bool enable){

	if(enable){
		/* setting the interrupt */
	    if( !writeRegister(INT_PIN_CFG,INT_PULSE_50US) ){ // setup interrupt, 50 us pulse
	        return -1;
	    }  
	    if( !writeRegister(INT_ENABLE,INT_RAW_RDY_EN) ){ // set to data ready
	        return -1;
	    }  
	}
	else{
	    if( !writeRegister(INT_ENABLE,INT_DISABLE) ){ // disable interrupt
	        return -1;
	    }  
	}

    // successful interrupt setup, return 0
    return 0; 
}


/* get accelerometer data given pointers to store the three values, return data as counts */
void MPU9250::getAccelCounts(int16_t* ax, int16_t* ay, int16_t* az){
    uint8_t buff[6];
    int16_t axx, ayy, azz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    ayy = (((int16_t)buff[2]) << 8) | buff[3];
    azz = (((int16_t)buff[4]) << 8) | buff[5];

    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;
}

/* get accelerometer data given pointers to store the three values */
void MPU9250::getAccel(float* ax, float* ay, float* az){
    int16_t accel[3];

    getAccelCounts(&accel[0], &accel[1], &accel[2]);

    *ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
    *ay = ((float) accel[1]) * _accelScale;
    *az = ((float) accel[2]) * _accelScale;
}

/* get gyro data given pointers to store the three values, return data as counts */
void MPU9250::getGyroCounts(int16_t* gx, int16_t* gy, int16_t* gz){
    uint8_t buff[6];
    int16_t gxx, gyy, gzz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(GYRO_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    gxx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    gyy = (((int16_t)buff[2]) << 8) | buff[3];
    gzz = (((int16_t)buff[4]) << 8) | buff[5];

    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz; // transform axes
    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}

/* get gyro data given pointers to store the three values */
void MPU9250::getGyro(float* gx, float* gy, float* gz){
    int16_t gyro[3];

    getGyroCounts(&gyro[0], &gyro[1], &gyro[2]);

    *gx = ((float) gyro[0]) * _gyroScale; // typecast and scale to values
    *gy = ((float) gyro[1]) * _gyroScale;
    *gz = ((float) gyro[2]) * _gyroScale;
}

/* get magnetometer data given pointers to store the three values, return data as counts */
void MPU9250::getMagCounts(int16_t* hx, int16_t* hy, int16_t* hz){
    uint8_t buff[7];
    _useSPIHS = true; // use the high speed SPI for data readout

    // read the magnetometer data off the external sensor buffer
    readRegisters(EXT_SENS_DATA_00,sizeof(buff),&buff[0]);

    if( buff[6] == 0x10 ) { // check for overflow
        *hx = (((int16_t)buff[1]) << 8) | buff[0];  // combine into 16 bit values
        *hy = (((int16_t)buff[3]) << 8) | buff[2];
        *hz = (((int16_t)buff[5]) << 8) | buff[4];
    }
    else{
        *hx = 0;  
        *hy = 0;
        *hz = 0;
    }
}

/* get magnetometer data given pointers to store the three values */
void MPU9250::getMag(float* hx, float* hy, float* hz){
    int16_t mag[3];

    getMagCounts(&mag[0], &mag[1], &mag[2]);

    *hx = ((float) mag[0]) * _magScaleX; // typecast and scale to values
    *hy = ((float) mag[1]) * _magScaleY;
    *hz = ((float) mag[2]) * _magScaleZ;
}

/* get temperature data given pointer to store the value, return data as counts */
void MPU9250::getTempCounts(int16_t* t){
    uint8_t buff[2];
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(TEMP_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    *t = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit value and return
}

/* get temperature data given pointer to store the values */
void MPU9250::getTemp(float* t){
    int16_t tempCount;

    getTempCounts(&tempCount);

    *t = (( ((float) tempCount) - _tempOffset )/_tempScale) + _tempOffset;
}

/* get accelerometer and gyro data given pointers to store values, return data as counts */
void MPU9250::getMotion6Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz){
    uint8_t buff[14];
    int16_t axx, ayy, azz, gxx, gyy, gzz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    ayy = (((int16_t)buff[2]) << 8) | buff[3];
    azz = (((int16_t)buff[4]) << 8) | buff[5];

    gxx = (((int16_t)buff[8]) << 8) | buff[9];
    gyy = (((int16_t)buff[10]) << 8) | buff[11];
    gzz = (((int16_t)buff[12]) << 8) | buff[13];

    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;

    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}

/* get accelerometer and gyro data given pointers to store values */
void MPU9250::getMotion6(float* ax, float* ay, float* az, float* gx, float* gy, float* gz){
    int16_t accel[3];
    int16_t gyro[3];

    getMotion6Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2]);

    *ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
    *ay = ((float) accel[1]) * _accelScale;
    *az = ((float) accel[2]) * _accelScale;

    *gx = ((float) gyro[0]) * _gyroScale;
    *gy = ((float) gyro[1]) * _gyroScale;
    *gz = ((float) gyro[2]) * _gyroScale;
}

/* get accelerometer, gyro and temperature data given pointers to store values, return data as counts */
void MPU9250::getMotion7Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* t){
    uint8_t buff[14];
    int16_t axx, ayy, azz, gxx, gyy, gzz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    ayy = (((int16_t)buff[2]) << 8) | buff[3];
    azz = (((int16_t)buff[4]) << 8) | buff[5];

    *t = (((int16_t)buff[6]) << 8) | buff[7];

    gxx = (((int16_t)buff[8]) << 8) | buff[9];
    gyy = (((int16_t)buff[10]) << 8) | buff[11];
    gzz = (((int16_t)buff[12]) << 8) | buff[13];

    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;

    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}

/* get accelerometer, gyro, and temperature data given pointers to store values */
void MPU9250::getMotion7(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* t){
    int16_t accel[3];
    int16_t gyro[3];
    int16_t tempCount;

    getMotion7Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &tempCount);

    *ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
    *ay = ((float) accel[1]) * _accelScale;
    *az = ((float) accel[2]) * _accelScale;

    *gx = ((float) gyro[0]) * _gyroScale;
    *gy = ((float) gyro[1]) * _gyroScale;
    *gz = ((float) gyro[2]) * _gyroScale;

    *t = (( ((float) tempCount) - _tempOffset )/_tempScale) + _tempOffset; 
}

/* get accelerometer, gyro and magnetometer data given pointers to store values, return data as counts */
void MPU9250::getMotion9Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz){
    uint8_t buff[21];
    int16_t axx, ayy, azz, gxx, gyy, gzz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    ayy = (((int16_t)buff[2]) << 8) | buff[3];
    azz = (((int16_t)buff[4]) << 8) | buff[5];

    gxx = (((int16_t)buff[8]) << 8) | buff[9];
    gyy = (((int16_t)buff[10]) << 8) | buff[11];
    gzz = (((int16_t)buff[12]) << 8) | buff[13];

    *hx = (((int16_t)buff[15]) << 8) | buff[14];  
    *hy = (((int16_t)buff[17]) << 8) | buff[16];
    *hz = (((int16_t)buff[19]) << 8) | buff[18];

    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;

    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}

/* get accelerometer, gyro, and magnetometer data given pointers to store values */
void MPU9250::getMotion9(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz){
    int16_t accel[3];
    int16_t gyro[3];
    int16_t mag[3];

    getMotion9Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &mag[0], &mag[1], &mag[2]);

    *ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
    *ay = ((float) accel[1]) * _accelScale;
    *az = ((float) accel[2]) * _accelScale;

    *gx = ((float) gyro[0]) * _gyroScale;
    *gy = ((float) gyro[1]) * _gyroScale;
    *gz = ((float) gyro[2]) * _gyroScale;

    *hx = ((float) mag[0]) * _magScaleX;
    *hy = ((float) mag[1]) * _magScaleY;
    *hz = ((float) mag[2]) * _magScaleZ;
}

/* get accelerometer, magnetometer, and temperature data given pointers to store values, return data as counts */
void MPU9250::getMotion10Counts(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* hx, int16_t* hy, int16_t* hz, int16_t* t){
    uint8_t buff[21];
    int16_t axx, ayy, azz, gxx, gyy, gzz;
    _useSPIHS = true; // use the high speed SPI for data readout

    readRegisters(ACCEL_OUT, sizeof(buff), &buff[0]); // grab the data from the MPU9250

    axx = (((int16_t)buff[0]) << 8) | buff[1];  // combine into 16 bit values
    ayy = (((int16_t)buff[2]) << 8) | buff[3];
    azz = (((int16_t)buff[4]) << 8) | buff[5];

    *t = (((int16_t)buff[6]) << 8) | buff[7];

    gxx = (((int16_t)buff[8]) << 8) | buff[9];
    gyy = (((int16_t)buff[10]) << 8) | buff[11];
    gzz = (((int16_t)buff[12]) << 8) | buff[13];

    *hx = (((int16_t)buff[15]) << 8) | buff[14];
    *hy = (((int16_t)buff[17]) << 8) | buff[16];
    *hz = (((int16_t)buff[19]) << 8) | buff[18];

    *ax = tX[0]*axx + tX[1]*ayy + tX[2]*azz; // transform axes
    *ay = tY[0]*axx + tY[1]*ayy + tY[2]*azz;
    *az = tZ[0]*axx + tZ[1]*ayy + tZ[2]*azz;

    *gx = tX[0]*gxx + tX[1]*gyy + tX[2]*gzz;
    *gy = tY[0]*gxx + tY[1]*gyy + tY[2]*gzz;
    *gz = tZ[0]*gxx + tZ[1]*gyy + tZ[2]*gzz;
}

void MPU9250::getMotion10(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, float* t){
    int16_t accel[3];
    int16_t gyro[3];
    int16_t mag[3];
    int16_t tempCount;

    getMotion10Counts(&accel[0], &accel[1], &accel[2], &gyro[0], &gyro[1], &gyro[2], &mag[0], &mag[1], &mag[2], &tempCount);

    *ax = ((float) accel[0]) * _accelScale; // typecast and scale to values
    *ay = ((float) accel[1]) * _accelScale;
    *az = ((float) accel[2]) * _accelScale;

    *gx = ((float) gyro[0]) * _gyroScale;
    *gy = ((float) gyro[1]) * _gyroScale;
    *gz = ((float) gyro[2]) * _gyroScale;

    *hx = ((float) mag[0]) * _magScaleX;
    *hy = ((float) mag[1]) * _magScaleY;
    *hz = ((float) mag[2]) * _magScaleZ;

    *t = (( ((float) tempCount) - _tempOffset )/_tempScale) + _tempOffset; 
}


/* writes a byte to MPU9250 register given a register address and data */
bool MPU9250::writeRegister(uint8_t subAddress, uint8_t data){
    uint8_t buff[1];

    /* write data to device */
    if( _useSPI ){

    	// Teensy 3.0 || Teensy 3.1/3.2
		#if defined(__MK20DX128__) || defined(__MK20DX256__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)){
		        SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI.transfer(subAddress); // write the register address
		        SPI.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}

    	#endif

        // Teensy 3.5 || Teensy 3.6 
		#if defined(__MK64FX512__) || defined(__MK66FX1M0__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)||(_mosiPin == MOSI_PIN_28)){
		        SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI.transfer(subAddress); // write the register address
		        SPI.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_0)||(_mosiPin == MOSI_PIN_21)){
		        SPI1.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI1.transfer(subAddress); // write the register address
		        SPI1.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI1.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_44)||(_mosiPin == MOSI_PIN_52)){
		    	SPI2.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI2.transfer(subAddress); // write the register address
		        SPI2.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI2.endTransaction(); // end the transaction	
	    	}

    	#endif

        // Teensy LC 
		#if defined(__MKL26Z64__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)){
		        SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI.transfer(subAddress); // write the register address
		        SPI.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_0)||(_mosiPin == MOSI_PIN_21)){
		        SPI1.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip
		        SPI1.transfer(subAddress); // write the register address
		        SPI1.transfer(data); // write the data
		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI1.endTransaction(); // end the transaction
	    	}

    	#endif
    }
    else{
      	i2c_t3(_bus).beginTransmission(_address); // open the device
      	i2c_t3(_bus).write(subAddress); // write the register address
      	i2c_t3(_bus).write(data); // write the data
      	i2c_t3(_bus).endTransmission();
    }
    delay(10); // need to slow down how fast I write to MPU9250

  	/* read back the register */
  	readRegisters(subAddress,sizeof(buff),&buff[0]);

  	/* check the read back register against the written register */
  	if(buff[0] == data) {
  		return true;
  	}
  	else{
  		return false;
  	}
}

/* reads registers from MPU9250 given a starting register address, number of bytes, and a pointer to store data */
void MPU9250::readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){

    if( _useSPI ){

    	// Teensy 3.0 || Teensy 3.1/3.2
		#if defined(__MK20DX128__) || defined(__MK20DX256__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}

    	#endif

        // Teensy 3.5 || Teensy 3.6 
		#if defined(__MK64FX512__) || defined(__MK66FX1M0__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)||(_mosiPin == MOSI_PIN_28)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_0)||(_mosiPin == MOSI_PIN_21)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI1.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI1.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI1.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI1.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI1.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_44)||(_mosiPin == MOSI_PIN_52)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI2.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI2.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI2.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI2.endTransaction(); // end the transaction
	    	}

    	#endif

        // Teensy LC 
		#if defined(__MKL26Z64__)

	    	if((_mosiPin == MOSI_PIN_11)||(_mosiPin == MOSI_PIN_7)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI.endTransaction(); // end the transaction
	    	}
	    	else if((_mosiPin == MOSI_PIN_0)||(_mosiPin == MOSI_PIN_21)){
		        // begin the transaction
		        if(_useSPIHS){
		            SPI1.beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        else{
		            SPI1.beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
		        }
		        digitalWriteFast(_csPin,LOW); // select the MPU9250 chip

		        SPI1.transfer(subAddress | SPI_READ); // specify the starting register address

		        for(uint8_t i = 0; i < count; i++){
		            dest[i] = SPI1.transfer(0x00); // read the data
		        }

		        digitalWriteFast(_csPin,HIGH); // deselect the MPU9250 chip
		        SPI1.endTransaction(); // end the transaction
	    	}

    	#endif
    }
    else{
        i2c_t3(_bus).beginTransmission(_address); // open the device
        i2c_t3(_bus).write(subAddress); // specify the starting register address
        i2c_t3(_bus).endTransmission(false);

        i2c_t3(_bus).requestFrom(_address, count); // specify the number of bytes to receive

        uint8_t i = 0; // read the data into the buffer
        while( i2c_t3(_bus).available() ){
            dest[i++] = i2c_t3(_bus).readByte();
        }
    }
}

/* writes a register to the AK8963 given a register address and data */
bool MPU9250::writeAK8963Register(uint8_t subAddress, uint8_t data){
	uint8_t count = 1;
	uint8_t buff[1];

	writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR); // set slave 0 to the AK8963 and set for write
	writeRegister(I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
	writeRegister(I2C_SLV0_DO,data); // store the data for write
	writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and send 1 byte

	// read the register and confirm
	readAK8963Registers(subAddress, sizeof(buff), &buff[0]);

	if(buff[0] == data) {
  		return true;
  	}
  	else{
  		return false;
  	}
}

/* reads registers from the AK8963 */
void MPU9250::readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest){

	writeRegister(I2C_SLV0_ADDR,AK8963_I2C_ADDR | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
	writeRegister(I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
	writeRegister(I2C_SLV0_CTRL,I2C_SLV0_EN | count); // enable I2C and request the bytes
	delayMicroseconds(100); // takes some time for these registers to fill
	readRegisters(EXT_SENS_DATA_00,count,dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers
}

/* gets the MPU9250 WHO_AM_I register value, expected to be 0x71 */
uint8_t MPU9250::whoAmI(){
    uint8_t buff[1];

    // read the WHO AM I register
    readRegisters(WHO_AM_I,sizeof(buff),&buff[0]);

    // return the register value
    return buff[0];
}

/* gets the AK8963 WHO_AM_I register value, expected to be 0x48 */
uint8_t MPU9250::whoAmIAK8963(){
    uint8_t buff[1];

    // read the WHO AM I register
    readAK8963Registers(AK8963_WHO_AM_I,sizeof(buff),&buff[0]);

    // return the register value
    return buff[0];
}

void MPU9250::selfTest(float * destination){
    Serial.begin(115200);
    delay(1000);
    Serial.println("self test start");
    delay(500);
    
    _pullups = I2C_PULLUP_EXT; // default to external pullups
    #if defined(__MK66FX1M0__) // Teensy 3.6
        if(_bus == 3) {
            _pins = I2C_PINS_56_57;
        }
        else if(_bus == 2) {
            _pins = I2C_PINS_3_4;
        }
        else if(_bus == 1) {
            _pins = I2C_PINS_37_38;
        }
        else{
            _pins = I2C_PINS_18_19;
            _bus = 0;
        }
        
    #endif
    // starting the I2C bus
    i2c_t3(_bus).begin(I2C_MASTER, 0, _pins, _pullups, _i2cRate);
    //#define SMPLRT_DIV       0x19
    //#define CONFIG           0x1A
    //#define GYRO_CONFIG      0x1B
    //#define ACCEL_CONFIG     0x1C
    //#define ACCEL_CONFIG2    0x1D
    uint8_t rawData[6] = {0, 0, 0, 0, 0, 0};
    uint8_t selfTest[6];
    int32_t gAvg[3] = {0}, aAvg[3] = {0}, aSTAvg[3] = {0}, gSTAvg[3] = {0};
    float factoryTrim[6];
    uint8_t FS = 0;
    
    writeRegister(SMPDIV, 0x00);    // Set gyro sample rate to 1 kHz
    writeRegister(CONFIG, 0x02);        // Set gyro sample rate to 1 kHz and DLPF to 92 Hz
    writeRegister(GYRO_CONFIG, FS<<3);  // Set full scale range for the gyro to 250 dps
    writeRegister(ACCEL_CONFIG2, 0x02); // Set accelerometer rate to 1 kHz and bandwidth to 92 Hz
    writeRegister(ACCEL_CONFIG, FS<<3); // Set full scale range for the accelerometer to 2 g
    
    // get average current values of gyro and acclerometer
    for( int ii = 0; ii < 200; ii++) {
        
        readRegisters(ACCEL_XOUT_H, 6, &rawData[0]);        // Read the six raw data registers into data array
        aAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
        aAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
        aAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
        
        readRegisters(GYRO_XOUT_H, 6, &rawData[0]);       // Read the six raw data registers sequentially into data array
        gAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
        gAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
        gAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
    }
    
    // Get average of 200 values and store as average current readings
    for (int ii =0; ii < 3; ii++) {
        aAvg[ii] /= 200;
        gAvg[ii] /= 200;
    }
    
    // Configure the accelerometer for self-test
    writeRegister(ACCEL_CONFIG, 0xE0); // Enable self test on all three axes and set accelerometer range to +/- 2 g
    writeRegister(GYRO_CONFIG,  0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
    delay(25);  // Delay a while to let the device stabilize
    
    // get average self-test values of gyro and acclerometer
    for( int ii = 0; ii < 200; ii++) {
        
        readRegisters(ACCEL_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers into data array
        aSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
        aSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
        aSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
        
        readRegisters(GYRO_XOUT_H, 6, &rawData[0]);  // Read the six raw data registers sequentially into data array
        gSTAvg[0] += (int16_t)(((int16_t)rawData[0] << 8) | rawData[1]) ;  // Turn the MSB and LSB into a signed 16-bit value
        gSTAvg[1] += (int16_t)(((int16_t)rawData[2] << 8) | rawData[3]) ;
        gSTAvg[2] += (int16_t)(((int16_t)rawData[4] << 8) | rawData[5]) ;
    }
    
    // Get average of 200 values and store as average self-test readings
    for (int ii =0; ii < 3; ii++) {
        aSTAvg[ii] /= 200;
        gSTAvg[ii] /= 200;
    }
    
    // Configure the gyro and accelerometer for normal operation
    writeRegister(ACCEL_CONFIG, 0x00);
    writeRegister(GYRO_CONFIG,  0x00);
    delay(25);  // Delay a while to let the device stabilize
    
    // Retrieve accelerometer and gyro factory Self-Test Code from USR_Reg
    readRegisters(SELF_TEST_X_ACCEL,1,&selfTest[0]); // X-axis accel self-test results
    readRegisters(SELF_TEST_Y_ACCEL,1,&selfTest[1]); // Y-axis accel self-test results
    readRegisters(SELF_TEST_Z_ACCEL,1,&selfTest[2]); // Z-axis accel self-test results
    readRegisters(SELF_TEST_X_GYRO,1,&selfTest[3]);  // X-axis gyro self-test results
    readRegisters(SELF_TEST_Y_GYRO,1,&selfTest[4]);  // Y-axis gyro self-test results
    readRegisters(SELF_TEST_Z_GYRO,1,&selfTest[5]);  // Z-axis gyro self-test results
    
    // Retrieve factory self-test value from self-test code reads
    factoryTrim[0] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[0] - 1.0) )); // FT[Xa] factory trim calculation
    factoryTrim[1] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[1] - 1.0) )); // FT[Ya] factory trim calculation
    factoryTrim[2] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[2] - 1.0) )); // FT[Za] factory trim calculation
    factoryTrim[3] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
    factoryTrim[4] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
    factoryTrim[5] = (float)(2620/1<<FS)*(pow( 1.01 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation
    
    // Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
    // To get percent, must multiply by 100
    for (int i = 0; i < 3; i++) {
        destination[i]   = 100.0*((float)(aSTAvg[i] - aAvg[i]))/factoryTrim[i] - 100.;   // Report percent differences
        destination[i+3] = 100.0*((float)(gSTAvg[i] - gAvg[i]))/factoryTrim[i+3] - 100.; // Report percent differences
    }
    
    Serial.print("x-axis self test: acceleration trim within : "); Serial.print(destination[0],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: acceleration trim within : "); Serial.print(destination[1],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: acceleration trim within : "); Serial.print(destination[2],1); Serial.println("% of factory value");
    Serial.print("x-axis self test: gyration trim within : "); Serial.print(destination[3],1); Serial.println("% of factory value");
    Serial.print("y-axis self test: gyration trim within : "); Serial.print(destination[4],1); Serial.println("% of factory value");
    Serial.print("z-axis self test: gyration trim within : "); Serial.print(destination[5],1); Serial.println("% of factory value");
    
    delay(1000);
    Serial.println("self test end");
    
    
}

// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void MPU9250::calibrate(float * dest1, float * dest2){
    
    Serial.begin(115200);
    delay(1000);
    Serial.println("calibrate start");
    delay(500);
    
    _pullups = I2C_PULLUP_EXT; // default to external pullups
#if defined(__MK66FX1M0__) // Teensy 3.6
    if(_bus == 3) {
        _pins = I2C_PINS_56_57;
    }
    else if(_bus == 2) {
        _pins = I2C_PINS_3_4;
    }
    else if(_bus == 1) {
        _pins = I2C_PINS_37_38;
    }
    else{
        _pins = I2C_PINS_18_19;
        _bus = 0;
    }
    
#endif
    // starting the I2C bus
    i2c_t3(_bus).begin(I2C_MASTER, 0, _pins, _pullups, _i2cRate);
    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
    writeRegister(PWR_MGMNT_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
    writeRegister(PWR_MGMNT_1, 0x01);
    writeRegister(PWR_MGMNT_2, 0x00);
    delay(200);
    
    // Configure device for bias calculation
    writeRegister(INT_ENABLE, 0x00);   // Disable all interrupts
    writeRegister(FIFO_EN, 0x00);      // Disable FIFO
    writeRegister(PWR_MGMNT_1, 0x00);   // Turn on internal clock source
    writeRegister(I2C_MST_CTRL, 0x00); // Disable I2C master
    writeRegister(USER_CTRL, 0x00);    // Disable FIFO and I2C master modes
    writeRegister(USER_CTRL, 0x0C);    // Reset FIFO and DMP
    delay(15);
    
    // Configure MPU6050 gyro and accelerometer for bias calculation
    writeRegister(CONFIG, 0x01);      // Set low-pass filter to 188 Hz
    writeRegister(SMPDIV, 0x00);  // Set sample rate to 1 kHz
    writeRegister(GYRO_CONFIG, 0x00);  // Set gyro full-scale to 250 degrees per second, maximum sensitivity
    writeRegister(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
    writeRegister(USER_CTRL, 0x40);   // Enable FIFO
    writeRegister(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
    writeRegister(FIFO_EN, 0x00);        // Disable gyro and accelerometer sensors for FIFO
    readRegisters(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};
        readRegisters(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;
    
    // Remove gravity from the z-axis accelerometer bias calculation
    if(accel_bias[2] > 0L){
        accel_bias[2] -= (int32_t) accelsensitivity;
    }
    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
    writeRegister(XG_OFFSET_H, data[0]);
    writeRegister(XG_OFFSET_L, data[1]);
    writeRegister(YG_OFFSET_H, data[2]);
    writeRegister(YG_OFFSET_L, data[3]);
    writeRegister(ZG_OFFSET_H, data[4]);
    writeRegister(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
    readRegisters(XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
    accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
    readRegisters(YA_OFFSET_H, 2, &data[0]);
    accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
    readRegisters(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
    writeRegister(XA_OFFSET_H, data[0]);
    writeRegister(XA_OFFSET_L, data[1]);
    writeRegister(YA_OFFSET_H, data[2]);
    writeRegister(YA_OFFSET_L, data[3]);
    writeRegister(ZA_OFFSET_H, data[4]);
    writeRegister(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;
    
    delay(1000);
    Serial.println("calibrate end");

}

void MPU9250::MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz){

    
    float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3];   // short name local variable for readability
    float norm;
    float hx, hy, _2bx, _2bz;
    float s1, s2, s3, s4;
    float qDot1, qDot2, qDot3, qDot4;
    
    // Auxiliary variables to avoid repeated arithmetic
    float _2q1mx;
    float _2q1my;
    float _2q1mz;
    float _2q2mx;
    float _4bx;
    float _4bz;
    float _2q1 = 2.0f * q1;
    float _2q2 = 2.0f * q2;
    float _2q3 = 2.0f * q3;
    float _2q4 = 2.0f * q4;
    float _2q1q3 = 2.0f * q1 * q3;
    float _2q3q4 = 2.0f * q3 * q4;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q1q4 = q1 * q4;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q2q4 = q2 * q4;
    float q3q3 = q3 * q3;
    float q3q4 = q3 * q4;
    float q4q4 = q4 * q4;
    
    // Normalise accelerometer measurement
    float sum = (ax * ax + ay * ay + az * az);
    norm = sqrtf(sum);
    if (norm == 0.0f) return; // handle NaN
    norm = 1.0f/norm;
    ax *= norm;
    ay *= norm;
    az *= norm;
    
    // Normalise magnetometer measurement
    norm = sqrtf(mx * mx + my * my + mz * mz);
    if (norm == 0.0f) return; // handle NaN
    norm = 1.0f/norm;
    mx *= norm;
    my *= norm;
    mz *= norm;
    
    // Reference direction of Earth's magnetic field
    _2q1mx = 2.0f * q1 * mx;
    _2q1my = 2.0f * q1 * my;
    _2q1mz = 2.0f * q1 * mz;
    _2q2mx = 2.0f * q2 * mx;
    hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
    hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
    _2bx = sqrtf(hx * hx + hy * hy);
    _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
    _4bx = 2.0f * _2bx;
    _4bz = 2.0f * _2bz;
    
    // Gradient decent algorithm corrective step
    s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
    s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
    s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
    s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
    norm = sqrtf(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
    norm = 1.0f/norm;
    s1 *= norm;
    s2 *= norm;
    s3 *= norm;
    s4 *= norm;
    
    // Compute rate of change of quaternion
    qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
    qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
    qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
    qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;
    
    // Integrate to yield quaternion
    q1 += qDot1 * deltat;
    q2 += qDot2 * deltat;
    q3 += qDot3 * deltat;
    q4 += qDot4 * deltat;
    norm = sqrtf(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);    // normalise quaternion
    norm = 1.0f/norm;
    q[0] = q1 * norm;
    q[1] = q2 * norm;
    q[2] = q3 * norm;
    q[3] = q4 * norm;

    
}

void MPU9250::getMotion14(float* ax, float* ay, float* az, float* gx, float* gy, float* gz, float* hx, float* hy, float* hz, float* t, float* qw, float* qx, float* qy, float* qz, uint32_t del){
    
    float grav = 9.81; //convert units to g's
    deltat = del;
    
    getMotion10(ax, ay, az, gx, gy, gz, hx, hy, hz, t);
    
    MadgwickQuaternionUpdate( *ax / grav, *ay / grav, *az / grav, *gx, *gy, *gz, *hy * 10, *hx * 10, *hz * 10 );
    
    *qw = q[0];
    *qx = q[1];
    *qy = q[2];
    *qz = q[3];
    
}

#endif
 
Why does it always flip the X and Y values on Gyro and Accel?

The attached extract from Invensense product specification document (p38) shows the Accel/Gyro axes are a different orientation to the Compass/Magnetometer. In Winer's code the X & Y axes of the magnetometer are swapped to bring both subsystems to the same alignment configuration ...

MPU9250 axes.JPG
 
Last edited:
If you look close at Brian's code for the getMotionXCounts and the accel/gyro counts he applies a transformation matrix to bring them into align. The matrix is actually defined in the .h file.
 
Hello everybody,
I'm the proud owner of a Teensy 3.6 and a Drotek MPU9250+MS5611 (Here the schematics).
I'm trying to use the Brian's library (very thanks for the huge work).
I've connected the two boards as follows (I've indicated the pins by their position on the board, where L and R indicate Left and Right and the number indicates the position of the pin, base 1. For example the first left is L1-GND):



TeensyDrotek MPU
L1-GND GND
R3-3.3VVDD
R8-SCL SCL (pulled up with a 2Kohm resistor)
R9-SDA SDA (pulled up with a 2Kohm resistor)
R4-IRQD0INT


I've tried to run the Interrupt_I2C sample, but it stops after printed few samples (couple of seconds at 100Hz).
What I noticed:
- The address is 0x69 instead of 0x68, even if I didn't modified the board. But I'll try to solder the switch to be sure that I'm not doing any mistake from this point of view.
- If I don't read the 10 IMU data, the interrupt function is called regularily 100Hz (I added a call to the led status change when getIMU is called to check it);

Now I wonder if I did something wrong.
The fact that I can read and print few samples means that the approach is almost good and probably I'm missing something stupid.
The Teensy receives correctly the interrupt (I can see it with an oscilloscope) and if I read less data (for example, accelerometers only) the program last longer (10-15 secs) without blocking.

The other strange thing is that if I use the basic sample with a single 10 values read and a shorter delay (10ms) it works fine but it stops anyway after many minutes (sometimes even one hour).

Did anyone experienced problems similar to the one I described?
Should I use the L15 3.3V power instead of the R3?
Is 2KOhm enough to pullup the SDA/SCL?
Wouldn't be better to use the FIFO to read faster?

Any suggestion?
teensy3.6drotek9250.jpg
 
Last edited:
It seems like something with the I2C communication. Are you able to get data from the other chip on the board, the MS5611?

You can run a program like this one to print out the I2C address of sensors connected to your Teensy:
https://github.com/nox771/i2c_t3/blob/master/examples/basic_scanner/basic_scanner.ino

It would be good to run that and verify that you're getting the expected addresses of the MPU-9250 and MS5611. Can you also post the code that you're using to test?

Thanks!
Brian
 
Hello all,

I'm using a SparkFun 9250 breakout board with a Teensy 3.2. This is a bit out of scope, since I'm not using the library here, but I thought it would be a good place to ask. I'm trying to read the magnetometer, but I am getting a NAK on the I2C. I have enabled the bypass in register 0x37, but I do not see any traffic on the secondary I2C. If I connect up to the aux I2C as a master, I can directly read the magnetometer (so the magnetometer is functioning). Has anyone seen behavior like this before? I am going to try another breakout board tomorrow to see if it is this one device with an issue. Searching for this issue brings up many many hits on Google saying "you have to enable the bypass to access the magnetometer", which I have already done.

Thanks,
Ryan
 
Hi Ryan,

I'm trying to understand better what you're attempting. Are you saying that:
1. You have the Teensy wired to communicate with the MPU-9250 over I2C
2. You can communicate with the MPU-9250 (i.e. it Acks its address)
3. You enable bypass mode via register 0x37
4. You then do not see an Ack from the AK8963C magnetometer's address, but you're still seeing an Ack from the MPU-9250 address?
 
Hi Ryan,

I'm trying to understand better what you're attempting. Are you saying that:
1. You have the Teensy wired to communicate with the MPU-9250 over I2C
2. You can communicate with the MPU-9250 (i.e. it Acks its address)
3. You enable bypass mode via register 0x37
4. You then do not see an Ack from the AK8963C magnetometer's address, but you're still seeing an Ack from the MPU-9250 address?

You are 100% correct this is what I am seeing.

To add the rest of my information in list format:

5. No traffic is seen on the aux I2C pins at any time (before or after enabling bypass mode).
6. Connecting directly to the aux I2C pins allows communication to the AK8963.

The solution with this board will just be to add an "External bypass" aka jumper the primary I2C bus over to the aux I2C bus. I will update when I replace this board (it is hardwired in) if it was a one-off hardware problem, or if I am still experiencing the issue on the second board as well.
 
You are 100% correct this is what I am seeing.

To add the rest of my information in list format:

5. No traffic is seen on the aux I2C pins at any time (before or after enabling bypass mode).
6. Connecting directly to the aux I2C pins allows communication to the AK8963.

The solution with this board will just be to add an "External bypass" aka jumper the primary I2C bus over to the aux I2C bus. I will update when I replace this board (it is hardwired in) if it was a one-off hardware problem, or if I am still experiencing the issue on the second board as well.

Using the attached code, which borrows heavily from i2c_t3 basic_scanner.ino, I get an Ack from both 0x68 (the MPU-9250) and 0x0C (the magnetometer). I ran this code with a Teensy 3.2 with Pins 18 and 19 wired to the I2C SDA and SCL lines on my MPU-9250 breakout board (embedded masters v1.1). You may have to unplug and replug the USB to your breakout board after uploading this code - it forces a restart of the magnetometer, which is required since this code is not careful enough handling the switch to bypass mode.

Hopefully that helps.

Brian
 

Attachments

  • basic_scanner.ino
    2.2 KB · Views: 160
Using the attached code, which borrows heavily from i2c_t3 basic_scanner.ino, I get an Ack from both 0x68 (the MPU-9250) and 0x0C (the magnetometer). I ran this code with a Teensy 3.2 with Pins 18 and 19 wired to the I2C SDA and SCL lines on my MPU-9250 breakout board (embedded masters v1.1). You may have to unplug and replug the USB to your breakout board after uploading this code - it forces a restart of the magnetometer, which is required since this code is not careful enough handling the switch to bypass mode.

Hopefully that helps.

Brian

I ran the scan and it only found 0x68, so I'm going to go with a failure of some sort in the chip. I'll wire up another board and report back. Thanks again for the assistance.

Edit: Interesting, the new board I am having the same problem with. I read something about pin 1 on the 9250 needing to be connected to VDDIO and there was a breakout board that didn't make this connection, but I haven't seen any complaints about the Sparkfun boards. Maybe I'll contact them and see if anyone else has had this issue.
 
Last edited:
I had difficulty talking with the magnetometer and ended up using the MPU9150s internal slave controller to do the job. I configured one channel to trigger a conversion and another to gather the results. Then I just read the data along with the rest.

In the end it was a wasted effort because the data was of such poor quality. Not very many bits of useful range and way too much noise. The 9250 is supposed to be a little bit better.
 
Back
Top