Tilt compensation in Invensense MPU9150

Other Hardware Sensors
1

Hey All,

I'm at my wits' end with this sensor. I'm trying to get a tilt-compensated magnetic heading reading from an MPU 9150 (http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/IMU/PS-MPU-9150A.pdf). The current libraries for it enable the DMP functionality and give me access to both the quaternions generated by the DMP and raw magnetic values. I'm using the sample code to keep things simple for now (using "MPU6050_9Axis_MotionApps41.h" to get the magnetic data).

Here's the final code:

            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            mpu.dmpGetMag(mag, fifoBuffer);

                        
            float magComp[3];
            float heading;
            
            float phi = ypr[2];
            float theta = ypr[1];
            
            float mx = mag[1];
            float my = mag[0];
            float mz = -mag[2];
            
            heading = atan2((mz * sin(phi) - my * cos(phi)), mx * cos(theta) + my * sin(theta) * sin(phi) + mz * sin(theta) * cos(phi));

The last four lines are of consequence (imo):

the 3 m_ assignments translate the mag axes to match the accel+gyro (the x and y are rotated, and the z is inverted). The last line is directly from FreeScale's doc on tilt compensation: http://cache.freescale.com/files/sensors/doc/app_note/AN4248.pdf (equation 22).

The problem is that the resulting heading value seems to be greatly correlated with both pitch and roll, and doesn't seem to correspond to what I would consider correct magnetic directions.

Any ideas here?

2

Are you certain that the definitions for the pitch and roll angles are exactly the same as defined in the AN4248 application note? That would include the definition of a positive rotation.

You might be better off just using the corrected accelerometer and magnetometer readings, and implementing the code described in AN4248 (it is available for download).

3

That's a fair point, let me check into signs.

That said, I've tried implementing the algorithm described, both in straight integer form, and using floats and math.h, with no luck. Either there are bugs in the code (I definitely caught missing semicolons), or I'm doing something wrong (also quite possible).

4

So, for what it's worth, there seem to have been two issues.

One is that the sensor appears to have been damaged. I tried the same code on two different units (i had a breakout board and a built-in module) and the BoB had the issue of no tilt compensation at all. I think the mag readings on it were just noise, I'll check into it more.

Two was what jremington suggested - signs on axes. Everything is relative, of course, but signs are what's important. Here's code that takes signs into account to reconcile the FreeScale tilt-compensation code with the quaternions produced by DMP:

float magComp[3];
float heading;

float phi = -ypr[2];
float theta = ypr[1];

float mx = mag[1];
float my = mag[0];
float mz = mag[2];

heading = atan2(-(mz * sin(phi) - my * cos(phi)), mx * cos(theta) + my * sin(theta) * sin(phi) + mz * sin(theta) * cos(phi));

notice that i'm explicitly not flipping the z axis (though it is in the axis definitions) and am flipping the sign on phi. this seems to produce a correct, tilt-compensated output.