MPU-9250 Teensy Library

Will your library recognise the teensy SPI libs alternate pin commands, like SPI.setSCK(14) ?

Not the way it's currently written. That's why you can select other SPI buses and alternate sets of pins. But you can't mix and match currently. In other words, with the current library, you can select the alternate SPI0 bus of:
MOSI: pin 7
MISO: pin 8
SCK: pin 14

But you can't mix and match pins from the SPI0 bus, like:
MOSI: pin 11
MISO: pin 12
SCK: pin 14

I can look at whether I can re-arrange the code to allow it.
 
you didnt need to add an external led, you could of just told the mcu to assign SCK to an alternate pin and keep D13 as the led.. see SPI.setSCK(newPin); and reference newPin with the alternate SCK0 pin listed on your teensy card
 
I can look at whether I can re-arrange the code to allow it.

It's not a big deal, maybe consider it when in next major update. The advantage of 11/12/14 is it frees up the LED_BUILTIN on pin 13.
 
Last edited:
Has anyone tried implementing the wake-on-interrupt feature?

http://www.invensense.com/wp-content/uploads/2015/02/PS-MPU-9250A-01-v1.1.pdf page 30/31

6.1 Wake-on-Motion Interrupt
The MPU-9250 provides motion detection capability. A qualifying motion sample is one where the high passed sample from any axis has an absolute value exceeding a user-programmable threshold. The following flowchart explains how to configure the Wake-on-Motion Interrupt. For further details on individual registers, please refer to the MPU-9250 Registers Map and Registers Description document.
In order to properly enable motion interrupts, the INT pin should be connected to a GPIO on the system processor that is capable of waking up the system processor.

This would allow very low power applications, eg. deep sleeping the processor and waking by movement.
 
Aircraft in a long steady bank are actually a good way to "break" an AHRS and a reason why EKF's may offer better performance in high dynamic environments.

Brian

Brian,

This particular comment stands out for me. I have been testing an AHRS device (an EFIS) in flight that currently uses a BNO055 IMU. Today was in a Searey, We've also tested in a Beech, and a Super Drifter. Indeed, long descents, ascents, and banks do seem to result in temporary, but significant drift in the indication provided by the IMU. Do you know if this is a property of the fusion/filtering firmware?

I ask because I'm starting to feel convinced that the BNO055 is not meant for moving platforms having slowly changing orientations. As such I'm removing the BNO and going to try an MPU9250, in hopes that the driftiness can be resolved. Even if it means tinkering with the fusion and filtering.

Robb
 
Brian,

This particular comment stands out for me. I have been testing an AHRS device (an EFIS) in flight that currently uses a BNO055 IMU. Today was in a Searey, We've also tested in a Beech, and a Super Drifter. Indeed, long descents, ascents, and banks do seem to result in temporary, but significant drift in the indication provided by the IMU. Do you know if this is a property of the fusion/filtering firmware?

I ask because I'm starting to feel convinced that the BNO055 is not meant for moving platforms having slowly changing orientations. As such I'm removing the BNO and going to try an MPU9250, in hopes that the driftiness can be resolved. Even if it means tinkering with the fusion and filtering.

Robb

Hi Robb,

That sounds like AHRS fusion/filtering. I would try an EKF with IMU and GPS, regardless of the IMU sensor that you use. I think it would help during climbs/descents and steady banks.

Brian
 
Very interesting. From everything I read it seems the Extended Kalman Filter is superior to AHRS in all aspects, except it is more demending on processing power. If I've understood that right, and if teensy is powerful enough, why don't we see discussion of need for an EKF (with IMU and GPS) library for teensy?
 
I think that EKF filters are better for UAV's and aircraft where the forces in a long steady bank or long climb / descent can look to an IMU like it's not in motion. The EKF filter does require an additional sensor (GPS) and good data from that sensor. We've had bad GPS data corrupt our EKF solution on a rare occasion in the past; however, that's not anything that can't be worked around (i.e. checking for valid data before passing to the sensor). As a starting point, here's a very rough port of our 15 state EKF:
https://github.com/bolderflight/EKF15

I've had it working on a Teensy 3.6, but it crashes on a Teensy 3.2 at this time and I haven't had a chance yet to investigate why. Could use quite a lot of cleanup and optimization, especially to take advantage of the single precision floating point unit.

Brian
 
Hi Robb,

That sounds like AHRS fusion/filtering. I would try an EKF with IMU and GPS, regardless of the IMU sensor that you use. I think it would help during climbs/descents and steady banks.

Brian

Brian,

Do you mean take the BNO055 out of fusion mode and apply EKF to the raw data? I think if I have to do that, then perhaps its more economical to use something like an MPU-9250 vs paying for the unused fusion FW & MCU included with the BNO055. What are your thoughts?

Regarding your EKF15 library: Would you lose much precision if you were to refactor your doubles to floats? I think Paul S has mentioned that on the Teensy double is 64 bit and float is 32 bit and the Teensy FPU is 32 bit.

I found some doubles in my own code a while back. When I refactored to 32 bit floats, my math intensive processes sped up appreciably.

Robb
 
Last edited:
Brian,

Do you mean take the BNO055 out of fusion mode and apply EKF to the raw data? I think if I have to do that, then perhaps its more economical to use something like an MPU-9250 vs paying for the unused fusion FW & MCU included with the BNO055. What are your thoughts?

Regarding your EKF15 library: Would you lose much precision if you were to refactor your doubles to floats? I think Paul S has mentioned that on the Teensy double is 64 bit and float is 32 bit and the Teensy FPU is 32 bit.

I found some doubles in my own code a while back. When I refactored to 32 bit floats, my math intensive processes sped up appreciably.

Robb

Yes, I mean applying the EKF to the raw IMU data.

I think single precision floating point would work great, other than math involving the GPS latitude and longitude where the decreased precision may be noticed. At the University, we've considered some different approaches to maximize the use of single precision math, including the use of NED (North East Down) coordinates instead of LLA (Latitude, Longitude, Altitude).

Brian
 
Hi Brian,

First great job on the library.

Your EKF15 interests me. I use a simple EKF for the FreeIMU library I have been using and maintaining. You mentioned in your last post about GPS accuracy. I came across this library that you might be interested in NeoGPS.

Anyway. The primary reason for this post is I was trying to port you master mode method to retrieve data from the 9250 from within the FreeIMU library. I did post on GitHub but found a couple of problems that I fixed so I just closed the issue. For instance, for the life of me I could not get anything to work if I use Jeff Rowberg's library so I just used you code. I converted it to just straight wire calls and read/write registers. Got everything to work after that except can not get the AK8963 to enter fuse mode to pull the factory biases for the magnetometer. Also, when I try to print out getMotion9counts all values are zero. Something is not working but not sure where the problem could be. I checked the code against yours but did not see anything. Any hints. I could post code if you are willing to take a look at it.

Thanks
Mike
 
Hi Brian,

First great job on the library.

Your EKF15 interests me. I use a simple EKF for the FreeIMU library I have been using and maintaining. You mentioned in your last post about GPS accuracy. I came across this library that you might be interested in NeoGPS.

Anyway. The primary reason for this post is I was trying to port you master mode method to retrieve data from the 9250 from within the FreeIMU library. I did post on GitHub but found a couple of problems that I fixed so I just closed the issue. For instance, for the life of me I could not get anything to work if I use Jeff Rowberg's library so I just used you code. I converted it to just straight wire calls and read/write registers. Got everything to work after that except can not get the AK8963 to enter fuse mode to pull the factory biases for the magnetometer. Also, when I try to print out getMotion9counts all values are zero. Something is not working but not sure where the problem could be. I checked the code against yours but did not see anything. Any hints. I could post code if you are willing to take a look at it.

Thanks
Mike

Hi Mike,

Thanks! I saw your github issue and that you closed it.

Please post code, I'll look through it tomorrow and try to see what's going on.

Brian
 
Brian. Appreciate it. Just to let you know I am using Kris Winer's MPU9250 breakout. Just for ref I incorporated the 9250 code in the Jeff's MPU60x0 library since that essentially the base for the 6050, 9150, 9250 and 6500. I differentiate by using slightly different initialization calls.

Have a question for you. Do you have a EKF version that does not use GPS? Thinking of having a crack after I get this fixed.

Initialization code:
Code:
void MPU60X0::initialize9250MasterMode(){
	#include "AK8963.h"
	
	uint8_t buff[3];
    uint8_t data[7];
	float _magScaleX, _magScaleY, _magScaleZ;	
	bool test;
	
	//set dev address for magnetometer
	magDevAddr = 0x0C;
	
    // SPI Configuration
	if (bSPI) {
      SPI.begin();
    	pinMode(devAddr, OUTPUT);
    	digitalWrite(devAddr, HIGH);
		reset();
		delay(100);
		switchSPIEnabled(true);
		delay(1);
	} else {
		Wire.begin(400000);
		switchSPIEnabled(false);
	}
	
	setStandbyDisable();
	setSleepEnabled(false);
	
    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }

    // enable I2C master mode
    if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
        Serial.println("Master Mode Not Set");
    }

    // set the I2C bus speed to 400 kHz
    if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,I2C_MST_CLK) ){
        Serial.println("I2C Bus Speed Not Set");
    }

    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }

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

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

    // reset the AK8963
    writeAKRegister(AK8963_RA_CNTL2, PWR_RESET);

    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }

    // check the WHO AM I byte, expected value is 0x71 (decimal 113)
	readRegister(MPU60X0_RA_WHO_AM_I, 1,&buff[0]);
    if( buff[0] != 113 ){
        Serial.println("9250 Not Recognized");
    }

    // enable accelerometer and gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_2, SEN_ENABLE) ){
        Serial.println("Accel and Gyro not Enabled");
    }

    /* setup the accel and gyro ranges */
	setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);	// set gyro range to +/- 2000 deg/second
	setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);		// set accel range to +- 2g
	setDLPFMode(MPU60X0_DLPF_BW_98); 
	
    // enable I2C master mode
    if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
    	Serial.println("Master Mode Set");
    }

	// set the I2C bus speed to 400 kHz
	if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,MPU60X0_CLOCK_PLL_XGYRO) ){
		Serial.println("I2C Bus Set");
	}

	// check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
	readAKRegisters(AK8963_RA_WIA, sizeof(buff), &buff[0]);
	if(  buff[0] != 72 ){
        Serial.print(buff[0]); Serial.print(", ");Serial.println("AK does not match");
	}

    /* get the magnetometer calibration */

    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }
    delay(100); // long wait between AK8963 mode changes

    // set AK8963 to FUSE ROM access
    if( !writeAKRegister(AK8963_RA_ASAX, AK8963_MODE_FUSEROM)){
        Serial.println("FUSE ROM Access Not set");
    }

    delay(100); // long wait between AK8963 mode changes

    // read the AK8963 ASA registers and compute magnetometer scale factors
	readAKRegisters(AK8963_RA_ASAX, 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 
	//_magScaleX = buff[0];
	//_magScaleX = buff[1];
	//_magScaleX = buff[2];
	Serial.print(_magScaleX,8); Serial.print(", "); Serial.print(_magScaleY,8); 
	Serial.print(", "); Serial.println(_magScaleZ,8);
	
    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }
    delay(100); // long wait between AK8963 mode changes  

    // set AK8963 to 16 bit resolution, 100 Hz update rate
    if( !writeRegister(AK8963_RA_CNTL1, 0x16) ){
        Serial.println("Res Not Set");
    }
    delay(100); // long wait between AK8963 mode changes

    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }      

    // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
    readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);
	
    // successful init, return 0
	Serial.println("FINISHED");
}

Get Motion Functions

Code:
void MPU60X0::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;

    readRegister(MPU60X0_RA_ACCEL_XOUT_H, 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 MPU60X0::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]); // typecast and scale to values
    *ay = ((float) accel[1]);
    *az = ((float) accel[2]);

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

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

}

Register Reads/Writes

Code:
//========================================================
void MPU60X0::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest){
        Wire.beginTransmission(devAddr); // open the device
        Wire.write(subAddress); // specify the starting register address
        Wire.endTransmission(false);

        Wire.requestFrom(devAddr, count); // specify the number of bytes to receive
		
        uint8_t i = 0; // read the data into the buffer
        while( Wire.available() ){
            dest[i++] = Wire.read();
        }
}

/* writes a register to the AK8963 given a register address and data */
bool MPU60X0::writeAKRegister(uint8_t subAddress, uint8_t data){
	uint8_t count = 1;
	uint8_t buff[1];
	
	writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr); // set slave 0 to the AK8963 and set for write
	writeRegister(MPU60X0_RA_I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
	writeRegister(MPU60X0_RA_I2C_SLV0_DO, data); // store the data for write
	writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and send 1 byte

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

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


/* reads registers from the AK8963 */
void MPU60X0::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){
	
	writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
	writeRegister(MPU60X0_RA_I2C_SLV0_REG,  subAddress); // set the register to the desired AK8963 sub address
	writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and request the bytes
	delayMicroseconds(100); // takes some time for these registers to fill
	readRegister(MPU60X0_RA_EXT_SENS_DATA_00, count, dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers

}

bool MPU60X0::writeRegister(uint8_t subAddress, uint8_t data){
	uint8_t buff[1];

    Wire.beginTransmission(devAddr); // open the device
    Wire.write(subAddress); // write the register address
    Wire.write(data); // write the data
    Wire.endTransmission();
   
    delay(10); // need to slow down how fast I write to MPU9250
	
  	/* read back the register */
  	readRegister(subAddress,sizeof(buff),&buff[0]);

  	/* check the read back register against the written register */
  	if(buff[0] == data) {
  		return true;
  	}
  	else{
  		return false;
  	}
	
}
 
I came across this library that you might be interested in NeoGPS

I highly recommend NeoGPS by /Dev. It is by far the best (efficient, reliable and minimalist) GPS library I've used.

It's especially efficient when using GPS interrupts to drive the sketch (at 5 or 10Hz) but this also requires the supporting NeoHWSerial class as a drop in replacement for Arduino's HardwareSerial class.

However so far NeoGPS + HWSerial are only available for Uno and Mega2560. I will soon be looking to implement on teensy, but it will need /Dev to come up with a teensy compatible variants (or some other guru).
 
Thanks for the kind words!

To be clear, NeoGPS is fully operational on the Teensy. Just feed it characters from your favorite source: serial port, I2C, static array, whatever.

NeoHWSerial is just the Arduino HardwareSerial class with an added method: attachInterrupt. This allows you to process characters during the RX char interrupt. NeoGPS does not require NeoHWSerial or any serial library, but here are two NeoGPS examples that use NeoHWSerial. All other examples use the typical polling technique.

NeoHWSerial is only necessary when some part of an Arduino sketch blocks and a serial input buffer overflows. Specifically, SD writes were taking ~100ms, and this caused GPS characters to be dropped. I suspect that the Teensy would not have this problem, because it is faster, and it has enough RAM to have a larger SD write buffer and/or GPS input buffer.

I have also modified the SD library to "yield" while it's blocked at a write. The yield function can poll the serial port for data to prevent overflow during that time, instead of attaching to the RX char interrupt.

Again, NeoGPS is not dependent on either NeoHWSerial or SD yield. It's up to the system designer to make sure data from all sources is read and written in a timely fashion.

Regarding GPS errors, the Adafruit_GPS library does not validate the NMEA checksum. I don't know if you are using that library, but it allows invalid values to be parsed instead of rejected. NeoGPS validates the checksum.

Regarding GPS accuracy, NeoGPS provides 10 significant digits in lat/lon, and that accuracy is preserved by the Location class when it calculates Distance and Bearing. This is especially important at small distances.

There can also be errors introduced by incoherent fixes: data from the previous fix is mixed with data from the current fix. NeoGPS is the only library that produces coherent fixes: data from all sentences in a GPS update interval are correctly merged into one fix structure.

Cheers,
/dev
 
Brian. Appreciate it. Just to let you know I am using Kris Winer's MPU9250 breakout. Just for ref I incorporated the 9250 code in the Jeff's MPU60x0 library since that essentially the base for the 6050, 9150, 9250 and 6500. I differentiate by using slightly different initialization calls.

Have a question for you. Do you have a EKF version that does not use GPS? Thinking of having a crack after I get this fixed.

Initialization code:
Code:
void MPU60X0::initialize9250MasterMode(){
	#include "AK8963.h"
	
	uint8_t buff[3];
    uint8_t data[7];
	float _magScaleX, _magScaleY, _magScaleZ;	
	bool test;
	
	//set dev address for magnetometer
	magDevAddr = 0x0C;
	
    // SPI Configuration
	if (bSPI) {
      SPI.begin();
    	pinMode(devAddr, OUTPUT);
    	digitalWrite(devAddr, HIGH);
		reset();
		delay(100);
		switchSPIEnabled(true);
		delay(1);
	} else {
		Wire.begin(400000);
		switchSPIEnabled(false);
	}
	
	setStandbyDisable();
	setSleepEnabled(false);
	
    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }

    // enable I2C master mode
    if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
        Serial.println("Master Mode Not Set");
    }

    // set the I2C bus speed to 400 kHz
    if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,I2C_MST_CLK) ){
        Serial.println("I2C Bus Speed Not Set");
    }

    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }

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

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

    // reset the AK8963
    writeAKRegister(AK8963_RA_CNTL2, PWR_RESET);

    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }

    // check the WHO AM I byte, expected value is 0x71 (decimal 113)
	readRegister(MPU60X0_RA_WHO_AM_I, 1,&buff[0]);
    if( buff[0] != 113 ){
        Serial.println("9250 Not Recognized");
    }

    // enable accelerometer and gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_2, SEN_ENABLE) ){
        Serial.println("Accel and Gyro not Enabled");
    }

    /* setup the accel and gyro ranges */
	setFullScaleGyroRange(MPU60X0_GYRO_FS_2000);	// set gyro range to +/- 2000 deg/second
	setFullScaleAccelRange(MPU60X0_ACCEL_FS_2);		// set accel range to +- 2g
	setDLPFMode(MPU60X0_DLPF_BW_98); 
	
    // enable I2C master mode
    if( !writeRegister(MPU60X0_RA_USER_CTRL,I2C_MST_EN) ){
    	Serial.println("Master Mode Set");
    }

	// set the I2C bus speed to 400 kHz
	if( !writeRegister(MPU60X0_RA_I2C_MST_CTRL,MPU60X0_CLOCK_PLL_XGYRO) ){
		Serial.println("I2C Bus Set");
	}

	// check AK8963 WHO AM I register, expected value is 0x48 (decimal 72)
	readAKRegisters(AK8963_RA_WIA, sizeof(buff), &buff[0]);
	if(  buff[0] != 72 ){
        Serial.print(buff[0]); Serial.print(", ");Serial.println("AK does not match");
	}

    /* get the magnetometer calibration */

    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }
    delay(100); // long wait between AK8963 mode changes

    // set AK8963 to FUSE ROM access
    if( !writeAKRegister(AK8963_RA_ASAX, AK8963_MODE_FUSEROM)){
        Serial.println("FUSE ROM Access Not set");
    }

    delay(100); // long wait between AK8963 mode changes

    // read the AK8963 ASA registers and compute magnetometer scale factors
	readAKRegisters(AK8963_RA_ASAX, 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 
	//_magScaleX = buff[0];
	//_magScaleX = buff[1];
	//_magScaleX = buff[2];
	Serial.print(_magScaleX,8); Serial.print(", "); Serial.print(_magScaleY,8); 
	Serial.print(", "); Serial.println(_magScaleZ,8);
	
    // set AK8963 to Power Down
    if( !writeAKRegister(AK8963_RA_CNTL1, AK8963_MODE_POWERDOWN) ){
        Serial.println("AK Not Powered Down");
    }
    delay(100); // long wait between AK8963 mode changes  

    // set AK8963 to 16 bit resolution, 100 Hz update rate
    if( !writeRegister(AK8963_RA_CNTL1, 0x16) ){
        Serial.println("Res Not Set");
    }
    delay(100); // long wait between AK8963 mode changes

    // select clock source to gyro
    if( !writeRegister(MPU60X0_RA_PWR_MGMT_1, MPU60X0_CLOCK_PLL_XGYRO) ){
        Serial.println("Clock Source Not Set");
    }      

    // instruct the MPU9250 to get 7 bytes of data from the AK8963 at the sample rate
    readAKRegisters(AK8963_RA_HXL,sizeof(data),&data[0]);
	
    // successful init, return 0
	Serial.println("FINISHED");
}

Get Motion Functions

Code:
void MPU60X0::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;

    readRegister(MPU60X0_RA_ACCEL_XOUT_H, 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 MPU60X0::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]); // typecast and scale to values
    *ay = ((float) accel[1]);
    *az = ((float) accel[2]);

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

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

}

Register Reads/Writes

Code:
//========================================================
void MPU60X0::readRegister(uint8_t subAddress, uint8_t count, uint8_t* dest){
        Wire.beginTransmission(devAddr); // open the device
        Wire.write(subAddress); // specify the starting register address
        Wire.endTransmission(false);

        Wire.requestFrom(devAddr, count); // specify the number of bytes to receive
		
        uint8_t i = 0; // read the data into the buffer
        while( Wire.available() ){
            dest[i++] = Wire.read();
        }
}

/* writes a register to the AK8963 given a register address and data */
bool MPU60X0::writeAKRegister(uint8_t subAddress, uint8_t data){
	uint8_t count = 1;
	uint8_t buff[1];
	
	writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr); // set slave 0 to the AK8963 and set for write
	writeRegister(MPU60X0_RA_I2C_SLV0_REG,subAddress); // set the register to the desired AK8963 sub address
	writeRegister(MPU60X0_RA_I2C_SLV0_DO, data); // store the data for write
	writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and send 1 byte

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

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


/* reads registers from the AK8963 */
void MPU60X0::readAKRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest){
	
	writeRegister(MPU60X0_RA_I2C_SLV0_ADDR, magDevAddr | I2C_READ_FLAG); // set slave 0 to the AK8963 and set for read
	writeRegister(MPU60X0_RA_I2C_SLV0_REG,  subAddress); // set the register to the desired AK8963 sub address
	writeRegister(MPU60X0_RA_I2C_SLV0_CTRL, I2C_SLV0_EN | count); // enable I2C and request the bytes
	delayMicroseconds(100); // takes some time for these registers to fill
	readRegister(MPU60X0_RA_EXT_SENS_DATA_00, count, dest); // read the bytes off the MPU9250 EXT_SENS_DATA registers

}

bool MPU60X0::writeRegister(uint8_t subAddress, uint8_t data){
	uint8_t buff[1];

    Wire.beginTransmission(devAddr); // open the device
    Wire.write(subAddress); // write the register address
    Wire.write(data); // write the data
    Wire.endTransmission();
   
    delay(10); // need to slow down how fast I write to MPU9250
	
  	/* read back the register */
  	readRegister(subAddress,sizeof(buff),&buff[0]);

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

Hi Mike,

It looks like you're writing to the incorrect register for putting the AK8963 into fuse rom mode. Your code is attempting to write to register AK8963_RA_ASAX when it should be writing to AK8963_RA_CNTL1.

Hope that helps!
Brian
 
Thanks Brian. Can not tell you how many times I looked at those lines.

I made the change and it entered fuse mode without error this time. However, when I print the asa's I get 167, 0, 0 when I run your code I get 178,178,167. Something looks shifted. The values for accel, gyros and mags are all zero. I thought I checked the register values between the two but I will check it again.

By the way checked your UAV page and brought back memories when I looked at the downloads for wind tunnel and flight data. By trade I was a aero engineer. Haven't looked at that stuff in about 30 years. Now retired.

Thanks
Mike
 
Thanks Brian. Can not tell you how many times I looked at those lines.

I made the change and it entered fuse mode without error this time. However, when I print the asa's I get 167, 0, 0 when I run your code I get 178,178,167. Something looks shifted. The values for accel, gyros and mags are all zero. I thought I checked the register values between the two but I will check it again.

By the way checked your UAV page and brought back memories when I looked at the downloads for wind tunnel and flight data. By trade I was a aero engineer. Haven't looked at that stuff in about 30 years. Now retired.

Thanks
Mike

Not sure about the ASA values yet...but in the meantime, in getMotion9Counts you've commented out *ax, *ay, *az, *gx, *gy, and *gz, which are your outputs from that function and why your accel and gyro data is coming out as zeros.

Brian
 
Thanks Brian. Can not tell you how many times I looked at those lines.

Everything works fine not. Had to correct another stupid error I made in the getCounts function and what I was printing out for the asa biases.

Now to try and figure out why I could not do that within the scope of the original lib. Or may just leave it. Also next is try it with two 9250s once I finish the cleanup.

By the way checked your UAV page and brought back memories when I looked at the downloads for wind tunnel and flight data. By trade I was a aero engineer. Haven't looked at that stuff in about 30 years. Now retired.

Thanks
Mike
 
Last edited:
Thanks Brian. Can not tell you how many times I looked at those lines.

Everything works fine not. Had to correct another stupid error I made in the getCounts function and what I was printing out for the asa biases.

Now to try and figure out why I could not do that within the scope of the original lib. Or may just leave it. Also next is try it with two 9250s once I finish the cleanup.

By the way checked your UAV page and brought back memories when I looked at the downloads for wind tunnel and flight data. By trade I was a aero engineer. Haven't looked at that stuff in about 30 years. Now retired.

Thanks
Mike

Great! Glad it's working.

Wooo! Good to "meet" another aero engineer. In response to an earlier question - I don't have a non-gps EKF handy. I've had students design some with an IMU and airdata for a graduate course in GPS-denied navigation, but don't have any in a polished enough state. I keep wanting to put together an arduino library of sensor calibration tools (Gauss Newton) and navigation filters (AHRS, EKF's, wind estimators etc), but it's behind a couple of larger projects that I have on my plate, so it could be a while.
 
Thanks for the kind words! To be clear, NeoGPS is fully operational on the Teensy. Just feed it characters from your favorite source: serial port, I2C, static array, whatever.

Hey /dev, welcome to teensy world! Thanks for those clarifications on NeoGPS, it's great news. So now I have good reason to give it a whurl - - soon.

I'll post a new thread then, so as not to hijack this MPU9250 thread anymore :)

cheers++
 
Last edited:
Hi Brian

I pretty much making a collection of AHRS filters myself, really just for fun and learning. Have the two that Madgwick provided on his web site plus his grad descent that was in his paper (its slightly different). Made some corrections based on notes in DIYDrones. Also have two Kalman filters, 1 extended based on quaternions and the other from TKelectronics. The 15 state one interests me so I play around with that as time permits. Have three other projects going right now, this one on multiple IMUs, an autonomous rover, and moding a Onagofly quadcopter.

Anyway, I spoke too soon when I said everything was working. While I got data I am now having an issue with the getting data from the magnetometer. Accel and gyro are good, but the mag comes out zero or some fixed value. No errors from the initialization but noticed that whatever values are read from the reading the seven bytes from mag in the config is what is printed each time I print get9counts. Doesn't look like the mag is updating.

UPDATE: NEVER MIND - FOUND THE PROBLEM. Now all is good.
 
Last edited:
@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/MPU9250/tree/arduino-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
 
Back
Top