/MPU9250.cpp Secret

## MPU9250.cpp
#include "Arduino.h"
#include "MPU9250.h"
#include "SPI.h"     // SPI Library

/* MPU9250 object, input the SPI CS Pin */
MPU9250::MPU9250(uint8_t csPin){
    _csPin = csPin; // SPI CS Pin
    _spi = &SPI;    // default to SPI
    //_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 SPI bus */
MPU9250::MPU9250(uint8_t csPin, SPIClass *Spi){
    _csPin = csPin; // SPI CS Pin
    _spi = Spi;
    //_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];

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

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

    // begin SPI communication
    _spi->begin();

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

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

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

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

    // check the WHO AM I byte, expected value is 0x71 (decimal 113)
    if( whoAmI() != 113 ){
        return -4;
    }

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

    /* 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 -6;
            }
            _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 -7;
            }
            _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 -8;
            }
            _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 -9;
            }
            _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 -10;
            }
            _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 -11;
            }
            _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 -12;
            }
            _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 -13;
            }
            _gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
            break;
    }

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

    // 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;
    }

    /* 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];

    _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
    digitalWrite(_csPin,LOW); // select the MPU9250 chip
    _spi->transfer(subAddress); // write the register address
    _spi->transfer(data); // write the data
    digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
    _spi->endTransaction(); // end the transaction

    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){

    // begin the transaction
    if(_useSPIHS){
        _spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
    }
    else{
        _spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
    }
    digitalWrite(_csPin,LOW); // select the MPU9250 chip

    _spi->transfer(subAddress | SPI_READ); // specify the starting register address

    digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
    delayMicroseconds(1);
    digitalWrite(_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
    }

    digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
    _spi->endTransaction(); // end the transaction
}

/* 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];
}
	#include "Arduino.h"
	#include "MPU9250.h"
	#include "SPI.h" // SPI Library

	/* MPU9250 object, input the SPI CS Pin */
	MPU9250::MPU9250(uint8_t csPin){
	_csPin = csPin; // SPI CS Pin
	_spi = &SPI; // default to SPI
	//_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 SPI bus */
	MPU9250::MPU9250(uint8_t csPin, SPIClass *Spi){
	_csPin = csPin; // SPI CS Pin
	_spi = Spi;
	//_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];

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

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

	// begin SPI communication
	_spi->begin();

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

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

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

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

	// check the WHO AM I byte, expected value is 0x71 (decimal 113)
	if( whoAmI() != 113 ){
	return -4;
	}

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

	/* 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 -6;
	}
	_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 -7;
	}
	_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 -8;
	}
	_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 -9;
	}
	_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 -10;
	}
	_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 -11;
	}
	_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 -12;
	}
	_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 -13;
	}
	_gyroScale = 2000.0f/32767.5f * _d2r; // setting the gyro scale to 2000DPS
	break;
	}

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

	// 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;
	}

	/* 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];

	_spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3)); // begin the transaction
	digitalWrite(_csPin,LOW); // select the MPU9250 chip
	_spi->transfer(subAddress); // write the register address
	_spi->transfer(data); // write the data
	digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
	_spi->endTransaction(); // end the transaction

	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){

	// begin the transaction
	if(_useSPIHS){
	_spi->beginTransaction(SPISettings(SPI_HS_CLOCK, MSBFIRST, SPI_MODE3));
	}
	else{
	_spi->beginTransaction(SPISettings(SPI_LS_CLOCK, MSBFIRST, SPI_MODE3));
	}
	digitalWrite(_csPin,LOW); // select the MPU9250 chip

	_spi->transfer(subAddress \| SPI_READ); // specify the starting register address

	digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
	delayMicroseconds(1);
	digitalWrite(_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
	}

	digitalWrite(_csPin,HIGH); // deselect the MPU9250 chip
	_spi->endTransaction(); // end the transaction
	}

	/* 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];
	}