MPU9250.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
#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]; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment