Skip to content

Instantly share code, notes, and snippets.

/MPU9250.cpp Secret

Created Oct 23, 2017
Embed
What would you like to do?
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];
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.