Forum Rule: Always post complete source code & details to reproduce any issue!
Page 5 of 5 FirstFirst ... 3 4 5
Results 101 to 118 of 118

Thread: MPU-9250 Teensy Library

  1. #101
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    112
    Quote Originally Posted by mjs513 View Post
    @brtaylor. Do you mind if I convert the library for use with non-teensy boards?
    Hi Mike,

    I don't mind as long as you keep the header and license information in the derivative files. I would also recommend forking the code on github so it's easier to implement additions or modifications made to the library in the future.

    Alternatively, this has been something that I've been considering for a while, but I don't have non-teensy arduino devices to test against. I had some time this morning and added statements which should enable non-teensy boards to use the library via I2C or SPI:
    https://github.com/bolderflight/MPU9...rduino-generic

    If that seems like a better route to you, please let me know as well as letting me know if that code works for you plus what device you tested it against.

    Brian

  2. #102
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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

  3. #103
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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

  4. #104
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    112
    Quote Originally Posted by mjs513 View Post
    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

  5. #105
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    Let me work on getting SPI working first then I will submit the pull request.

    Mike

    UPDATE: Pull request completed.
    Last edited by mjs513; 04-05-2017 at 11:04 AM.

  6. #106
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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

  7. #107
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    112
    Quote Originally Posted by mjs513 View Post
    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

  8. #108
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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

  9. #109
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    Hi Brian

    Thought you would like to see how I am using your handiwork in the FreeIMU lib. Now I have to generate some sort of visualization for using two MPU.s

    https://youtu.be/u0w5qFWCbCE

    Think I found my next project after doing a little searching. http://umassgv.blogspot.com/2013/08/...rm-motion.html

    Planned hardware.
    (2) MPU-9250's - Onehorse boards
    (1) ESP-32 Development Board
    Last edited by mjs513; 04-08-2017 at 11:22 PM.

  10. #110
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    112
    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

  11. #111
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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

  12. #112
    Junior Member
    Join Date
    Feb 2017
    Posts
    7
    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

  13. #113
    Senior Member brtaylor's Avatar
    Join Date
    Mar 2016
    Location
    Portland, OR
    Posts
    112
    Quote Originally Posted by lawmate View Post
    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

  14. #114
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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 by mjs513; 05-05-2017 at 01:35 AM. Reason: Update

  15. #115
    Junior Member
    Join Date
    Feb 2017
    Posts
    7
    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

  16. #116
    Junior Member
    Join Date
    Feb 2017
    Posts
    7
    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
    brian.taylor@bolderflight.com
    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

  17. #117
    Senior Member ninja2's Avatar
    Join Date
    Aug 2016
    Location
    Adelaide, Australia
    Posts
    138
    Quote Originally Posted by lawmate View Post
    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 ...

    Click image for larger version. 

Name:	MPU9250 axes.JPG 
Views:	6 
Size:	61.4 KB 
ID:	10551
    Last edited by ninja2; 05-10-2017 at 08:19 PM.

  18. #118
    Senior Member
    Join Date
    Jul 2014
    Location
    New York
    Posts
    198
    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.

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •