New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

MPU6050_9Axis_MotionApps41.h #18

choolake opened this Issue Oct 10, 2012 · 8 comments


None yet
8 participants
Copy link

choolake commented Oct 10, 2012

In the MPU6050_6Axis_MotionApps20.h cause the quaternion to initialized in the power up direction. In other words the axis system of the wold frame depends on the power up orientation of the sensor. This is because that it does not have a magnetometer it cant make a fixed axis system. But with MPU6050_9Axis_MotionApps41.h this issue need to be solved since it uses the magnetometer but it does not use the magnetometer reading to correct the 6 -axis quaternion given by the MPU 6050. can you describe a way to correct the quaternion that is given by the MPU to not to depend on the power up direction but to depend on the magnetometer magmatic north.
thank you!


This comment has been minimized.

Copy link

jrowberg commented Oct 10, 2012

Hello @choolake, unfortunately I don't know the required 3D spatial math required to fuse the magnetometer data with the existing 6-axis fused quaternion data. I have been searching for this very same solution for quite some time, but so far I have not succeeded. InvenSense' own MotionFit platform with the MSP430 MCU does this, but it uses a linked library to do so, and the source code is not available for it.

The missing pieces are first to calculate a calibrated magnetic heading from the raw magnetometer values, and then to use that heading to establish absolute north and also to provide gyro yaw drift compensation over time. So, I know what has to be done, but I don't know how to do it.


This comment has been minimized.

Copy link

udawat commented Dec 31, 2012

Hi @jrowberg , for basic AHRS calculations, you might want to take a look at Mahony and Madgwick papers. They give a detailed view at how the 9 DoF quaternion algo is implemented. Also, I have been trying to include HMC5883L magnetometer reading into the MPU6050_9Axis_MotionApps41.h file but in vain.

I have modified some code to include HMC5883L in place of AK8975. Say,
at line 346, setI2CBypassEnabled(true); has been uncommented.

Line 350: Is it necessary to bring the magnetometer to power down mode? For HMC5883L, i changed the mode to HMC5883L_MODE_IDLE, i.e., 0x02.

Then, i wrote CONFIG A Register, CONFIG B register and set the magneto (HMC5883L) to Single Measurement Mode (basic initialization).

Line 354: Skipped the Fuse Access mode
Line 356: Read Factory Calibration data. I have changed it to:

DEBUG_PRINTLN(F("Read magnetometer heading..."));
int16_t magX, magY, magZ;
//mag -> getAdjustment(&asax, &asay, &asaz);
//I2Cdev::readBytes(0x1E, 0x10, 3, buffer);
I2Cdev::readBytes(0x1E, 0x03, 6, buffer);
I2Cdev::writeByte(0x1E, 0x02, 0x01 << (1 - 2 + 1));  // set Single Measurement Mode for HMC5883L

magX = (((int16_t)buffer[0]) << 8) | buffer[1];
magY = (((int16_t)buffer[4]) << 8) | buffer[5];
magZ = (((int16_t)buffer[2]) << 8) | buffer[3];

DEBUG_PRINT(F("Adjustment X/Y/Z = "));
DEBUG_PRINT(F(" / "));
DEBUG_PRINT(F(" / "));
float heading = atan2(magY, magX);
if(heading < 0) {
heading += 2 * M_PI;
DEBUG_PRINT(F("degree: "));
DEBUG_PRINTLN(heading * 180/M_PI);

And it is showing the accurate heading. (0 degrees at North).

Line 478: I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x9E); // for HMC5883L I2C Address is 0x1E.

What i do not understand is after line: 478
... MPU6050_RA_I2C_SLV0_REG, 0x01); and
... MPU6050_RA_I2C_SLV0_CTRL, 0xDA);

What to write in place of 0x01 and 0xDA?

Is this the correct way to get magnetometer data from the DMP example?


This comment has been minimized.

Copy link

muzhig commented May 24, 2013

Hey @jrowberg and @udawat, I've successfully configured MPU6050 to read HMC5883L heading data via aux I2C bus to ext_sens_data registers by this code (without DMP turned on):


if (hmc5883l.testConnection()) {
    hmc5883l.setMode(HMC5883L_MODE_CONTINUOUS); // must be fixed, because current version doesn't work at all - must use writeBits

    // X axis word
    mpu6050.setSlaveAddress(0, HMC5883L_DEFAULT_ADDRESS | 0x80); 
    // 0x80 turns 7th bit ON, according to datasheet, 7th bit controls Read/Write direction
    mpu6050.setSlaveRegister(0, HMC5883L_RA_DATAX_H);
    mpu6050.setSlaveControl(0, true, false, false, false, 2); // num, slave_enabled, word_byte_swap, write_mode, group_offset, data_length
    // implemented prev method to set whole SLV*_CTRL reg at once.

    // Y axis word
    mpu6050.setSlaveAddress(1, HMC5883L_DEFAULT_ADDRESS | 0x80);
    mpu6050.setSlaveRegister(1, HMC5883L_RA_DATAY_H);
    mpu6050.setSlaveControl(1, true, false, false, false, 2);

    // Z axis word
    mpu6050.setSlaveAddress(2, HMC5883L_DEFAULT_ADDRESS | 0x80);
    mpu6050.setSlaveRegister(2, HMC5883L_RA_DATAZ_H);
    mpu6050.setSlaveControl(2, true, false, false, false, 2);

} else {
    Serial.println("HMC5883L: FAILURE");

I think it could help for future updating MotionApps41 code to use this compass


This comment has been minimized.

Copy link

jrowberg commented May 24, 2013

This is great! Thanks! I will try to roll it into a new example project in the repo shortly.


This comment has been minimized.

Copy link

dingeldongel commented May 28, 2013

Hi, I am also trying to use your library with an combinaation of MPU6050 and HMC5883L. similar to a FreeIMU.
I allready tested some different Codes especially yours and the codes of Fabio for the FreeIMU.
@muzhig I would like to implement your code but dont know in which file and where. Could you please help me?

@jrowberg : Great library. very stable sensor values with nearly no drift of the yaw axis. I cant wait to see how it works together with the HMC5883L.

Thanks a lot for this.


This comment has been minimized.

Copy link

gonzo2 commented Feb 9, 2014

@muzhig, could you explain how to implement your code, @jrowberg, please can you update the libraries to include the HMC5883L, I'm getting crazy!, you did an extraordinary job with the I2C libraries, I've used several of them, now I'm stuck on this one. I'm quite newbie, So Libs + example code for arduino 9axis would be my dream


This comment has been minimized.

Copy link

guyl44 commented Jul 4, 2016

Hey, I was wondering - is there a working version of MPU6050_9Axis_MotionApps41.h that communicates with the HMC5883L? Even a semi-working version? Anything?

I realize this is an old thread/issue, but I thought I might as well give it a shot.


This comment has been minimized.

Copy link

paddywwoof commented Aug 5, 2016

I don't know how active this corner of this (very excellent) project is, but for what it's worth this is what I did and it might be a way to put 9Axis functionality into the library.

In the example MPU6050_DMP6.ino (line numbers approx because of non-relevant additions) I've copied the hmc5883l bits straight from the examples with that library (scale and measurement mode) and don't know if they're optimal. See also my comment about it settling in a slightly arbitrary direction.

EDIT: found previous suggestion (using rotation of vector with quoternion) was flawed, but this seems to work quite well.

> #include <hmc5883l.h>
> hmc5883l compass;
>     mpu.setI2CBypassEnabled(true);
>     compass = hmc5883l();
>     compass.SetScale(1.3); // Set the scale of the compass.
>     compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
>      MagnetometerScaled mags = compass.ReadScaledAxis();
>      float sr = sin(ypr[2]); float cr = cos(ypr[2]); // sin and cos of roll
>      float sp = sin(ypr[1]); float cp = cos(ypr[1]); // sin and cos of pitch
>      float x_mag =  -mags.XAxis * cp + mags.ZAxis * sp;
>      float y_mag = -mags.XAxis * sr * sp + mags.YAxis * cr - mags.ZAxis * sr * cp;
>      float compass_diff = atan2(y_mag, -x_mag) - ypr[0];
>      if (compass_diff > 0.25) { // i.e. major change such as from pi to -pi
>        compass_offset = compass_diff;
>      }
>      else {
>        compass_offset = compass_offset * (1.0 - SMOOTH_F) + compass_diff * SMOOTH_F; 
>      }

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment