MPU 6050 roll function of pitch?

[bk4nt]

Hello,

I'm using MPU6050_6Axis_MotionApps20.h plus main lines of MPU6050_DMP6 to get roll out of the MPU6050. That works fine as long as pitch = zero. Code being that of the example:

      mpu.getFIFOBytes(buffer_fifo, mpu_packet_size);

      Quaternion  sample_q;        // [w, x, y, z]         quaternion container
      VectorFloat sample_gravity;  // [x, y, z]            gravity vector
      float       sample_ypr[3];   // [yaw, pitch, roll]   yaw/pitch/roll container

      mpu.dmpGetQuaternion(&sample_q, buffer_fifo);
      mpu.dmpGetGravity(&sample_gravity, &sample_q);
      mpu.dmpGetYawPitchRoll(sample_ypr, &sample_q, &sample_gravity);

Callibration is done, using offsets provided by IMU_ZERO sketch (adding extra setX/YAccelOffset lines, but that made no difference):

    mpu.setXGyroOffset(43);
    mpu.setYGyroOffset(-17);
    mpu.setZGyroOffset(-28);
    mpu.setXAccelOffset(-3108); // Added...
    mpu.setYAccelOffset(1079); // Added...
    mpu.setZAccelOffset(1443);

    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();

Works, but when pitch is different than zero, roll gets affected. For example, extrem, if pitch (MPU6050 chip orientation) is 90°, the roll output of MPU6050 is some 80-85° for 45° chip roll.

With a roll of 24°, if pitch varies between +/-30°, roll output shifts from 24° to 27-28°.
With a roll of 36°, if pitch varies between +/-30°, roll output shifts from 36° to 42-43°.

Do I miss use MPU6050_6Axis_MotionApps20.h and MPU6050 or is any exta math required when both roll and pitch are different from zero?

Best regards

[ZHomeSlice]

I have updated the MPU6050_6Axis_MotionApps20.h code to include a very fast PID based calibration routine. Make a backup of the old MPU6050 library just to be safe.
The example code has the calibration routine and provides offsets for you to use.

you may want to try these equations out:

// Included with the library Gravity only Based pitch and roll
void GetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
	#ifdef USE_OLD_GETYAWPITCHROLL
	// yaw: (about Z axis)
	data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
	// pitch: (nose up/down, about Y axis)
	data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
	// roll: (tilt left/right, about X axis)
	data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
	#else 
	// yaw: (about Z axis)
	data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
	// pitch: (nose up/down, about Y axis)
	data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
	// roll: (tilt left/right, about X axis)
	data[2] = atan2(gravity -> y , gravity -> z);
	if (gravity -> z < 0) {
		if(data[1] > 0) {
			data[1] = PI - data[1];
			} else {
			data[1] = -PI - data[1];
		}
	}
	#endif

}
// New and a possible new addition to this library. Try it out.
// DMP integrated Quaternion  which I believe has Gyro and gravity based Pitch and Roll
void GetYawPitchRoll(float *data, Quaternion *q) {
	data[0] = atan2(2.0f * (q -> x*q -> y + q -> w * q -> z), q -> w * q -> w + q -> x * q -> x - q -> y * q -> y - q -> z * q -> z);
	data[1] = -asin(2.0f * (q -> x * q -> z - q -> w * q -> y));
	data[2]  = atan2(2.0f * (q -> w * q -> x + q -> y * q -> z), q -> w * q -> w - q -> x * q -> x - q -> y * q -> y + q -> z * q -> z);
	
}

The first function has 2 options each react differently to the use of acceleration and gravity with unique output ranges.

The second function (Not in the mpu6050.c code) uses the DMP Quaternion only. I believe the DMP already integrates the acceleration values with the Gyro portion for the Quaternion calculations that are used with Pitch and Roll so I believe this to be the most accurate way to handle pitch and roll.
Yaw has no other source to prevent drift completely until we can figure out the best way to integrate a magnetometer with the values.

Each of these functions generates different value ranges like pitch =+-90 while yaw and roll are -+180 (on the second function).
See what fits.

In addition to the new calibration routines, I have added the code for MPU6050_6Axis_MotionApps_V6_12.h which uses the latest instance of the DMP firmware. Although it is bigger it contains an auto-calibration routine for gyro which further stabilizes drift after several seconds of no motion.

Z

[bk4nt]

Sorry, I tested all this, but the results are similar. So I left aside my own code and flashed only MPU6050_DMP6_using_DMP_V6.12.ino. Which works, as long only either pitch or roll changes. As soon as both picth and roll are changed, they seem to affect each other also.

I've added quaternion dump, to see what happens there. Those results seem already affected. For aprox 39° roll, I get q.x = -0.34. If I then change pitch also, I do see q.x varing according to q.y sign and value. For negative q.y values, q.x increases. For positive q.y values, q.x decreases. Or if quaternion is correct, something is wrong with the maths?

In my hardware or mechanical setup, q.x changes for roll, q.y changes for pitch. There also, I see the same, if only q.x or q.y change, outputs look correct.

Best regards

   #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
        mpu.dmpGetGravity(&gravity, &q);
       // mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
       ypr[0] = atan2(2.0f * (q.x*q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z);
        ypr[1] = -asin(2.0f * (q.x * q.z - q.w * q.y));
        ypr[2]  = atan2(2.0f * (q.w * q.x + q.y * q.z), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z);


        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[2] * 180/M_PI);

        Serial.print("\t");
        Serial.print(q.w);
        Serial.print("\t");
        Serial.print(q.x);
        Serial.print("\t");
        Serial.print(q.y);
        Serial.print("\t");
        Serial.print(q.z);

As output example:

      ypr	-24.24	-28.64	0.80	0.95	-0.05	-0.24	-0.20
      ypr	-24.24	-28.64	0.81	0.95	-0.05	-0.24	-0.20
      ypr	-24.24	-28.64	0.81	0.95	-0.05	-0.24	-0.20

[bk4nt]

Sorry, I'm very new to quaternions and such subjects.

I now found and used QuatView to compare its outputs with MPU6050_DMP6_using_DMP_V6.12.ino
QuatView is here : http://www.tobynorris.com/work/prog/csharp/quatview/

For QuatView, 45° roll, 30° pitch, 0 yaw, Euler/Degrees, the quaternion shall be 0,89 / 0,37 / 0,24 / 0,1 (rounded)

Using DMP_V6.12.ino, I do read 49° roll, 21° pitch for 0,89 / 0,37 / 0,24 / 0,1

Shouldn't the readings be the same for the same quaternion? Is this the difference I noticed?

[ZHomeSlice]

If I'm understanding you correctly you are saying the Euler angles are incorrect?
The code I shared was not for Euler angles but instead, they are for yaw pitch and roll values.
your QuatView program uses Eular values.
I have not tested the Euler equations.

            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);

the function for Euler:

uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
    return 0;
}

you may want to change your code to this to test it

  #ifdef OUTPUT_READABLE_YAWPITCHROLL
        // display Euler angles in degrees
        mpu.dmpGetQuaternion(&q, fifoBuffer);
       //mpu.dmpGetGravity(&gravity, &q);
       // mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
       ypr[0] = atan2(2.0f * q.x * q.y - 2.0f * q.w * q.z, 2.0f * q.w * q.w + 2.0f * q.x * q.x - 1.0f);   // psi
//       ypr[0] = atan2(2.0f * (q.x*q.y + q.w * q.z), q.w * q.w + q.x * q.x - q.y * q.y - q.z * q.z);

        ypr[1] = -asin(2.0f * q.x * q.z + 2.0f * q.w * q.y);                              // theta
//        ypr[1] = -asin(2.0f * (q.x * q.z - q.w * q.y));

        ypr[2] = atan2(2.0f * q.y * q.z - 2.0f * q.w * q.x, 2.0f * q.w * q.w + 2.0f * q.z * q.z - 1.0f);   // phi
//        ypr[2] = atan2(2.0f * (q.w * q.x + q.y * q.z), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z);


        Serial.print("ypr\t");
        Serial.print(ypr[0] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[1] * 180/M_PI);
        Serial.print("\t");
        Serial.print(ypr[2] * 180/M_PI);

        Serial.print("\t");
        Serial.print(q.w);
        Serial.print("\t");
        Serial.print(q.x);
        Serial.print("\t");
        Serial.print(q.y);
        Serial.print("\t");
        Serial.print(q.z);

Z

[bk4nt]

I clarify again my needs and what I don't succeed to do.

I would like to record the roll data of a moving object. That object pitch and yaw change also.

I'm now using unchanged original MPU6050_DMP6_using_DMP_V6.12.ino example to read pitch/roll data. And I now have a better test bed but the roll results I get out are still incorrect as soon as pitch changes also. Using that example code to read out MPU6050 roll and pich angles:

Step 1, I change roll to any non zero and high value, roll readout is now 34.70°.

Step 2, I change pitch also to any non zero and high value, pitch readout is now 25.50°;
now with that pitch and unchanged roll, the roll readout shifted away to 38.30°.

After step 2 and pitch change, I was expecting to see the roll readout still at 34.70, not at 38.30°.

And my concern or question is why does the roll output shift away when pitch is non zero?

[ZHomeSlice]

If I'm understanding you correctly you are saying that the values that are generated from the pre-compiled Digital Motion Processing (DMP) code are generating invalid Quaternion values. as any change on the Pitch alters the roll values and vice versa. This is with clean calibration where when the MPU is set flat you are getting close to zero on all acceleration and gyro values after removing gravity from Z acceleration.
You have indicated that you have proven this with alternate external Quaternion to YPR Calculator and its values are also incorrect.
Does this analysis sound correct?

If it does, I must refer you to TDK InvenSense There development team created the DMP firmware that generated the Quaternion values and I have no control as to how the code determines the Quaternion values.

If this is not correct. and you believe an error in the math is the problem I am sorry I do not understand the Quaternion math enough to correct any Quaternion mathematical errors we may have generated in our attempt to bring you this library. I have copied others works when it comes to Quaternion math (the second Function) or like Jeffs function that uses Acceleration and gravity only to do the math for yaw and pitch so Quaternion values are not used.

I have attempted to determine the values are close to my understanding of what they should be but I am not a mathematician.

If you or anyone watching this, have a suggestion or a correction to the math I would be grateful and would like to add your version to the library with notation as to how it corrects the problem/error.

If there is anything I can help further figure this out I will, I have enjoyed working on this project of Jeffs as I know he has also. We would like to try to further help you if we can.

A Suggestion would be to post your complete code so I can test it on my rig. Maby it is simple to resolve. But I am not able to determine how to proceed at this time.
Z

[bk4nt]

any change on the Pitch alters the roll values and vice versa

Yes, correct, this is what I get with my MPU6050 chip and the Arduino libs, including the latest MPU6050_DMP6_using_DMP_V6.12.ino example code.

A Suggestion would be to post your complete code so I can test it on my rig

Just use MPU6050_DMP6_using_DMP_V6.12.ino example code to check or reproduce what I do get. If my MPU6050 isn't bad, you shall get the same results.

1. lay your chip flat, start MPU6050_DMP6_using_DMP_V6.12.ino, wait for calibration end
2. readout is now 0 / 0 for roll and pitch
3. increase the roll to some 30-40°, record the roll displayed by the sketch
4. now add pitch, some 20-30°, and see what the roll now is, shall be different from what was recorded on 3)

---

I thought the math could wrong. But I checked many examples and all do similar math to get roll/pitch from the quaternion. So this shall not be the issue.

---

What I've done with QuatView was wrong, I was told by its author. Such comparison is to be done again. I had the idea to compare QuatView output, an alternate tool, to MPU6050_DMP6_using_DMP_V6.12.ino output. This might allow to compare Euler angles and quaternions and maybe help to find out if it is any math or overflow issue or it it is a DMP code issue.

I was told following about my previous comparisons with QuatView: "The first set of numbers you gave with Euler angles of (YPR=45,30,0) gave a quaternion with values of (0.89,0.37,0.24,0.10). This corresponds to the rotation order of 123. When I change the rotation sequence in Quatview to be 321 instead of 123, then I get the same Euler angles that you get from your MPU6050. So perhaps your MPU6050 is doing the rotations in the order of 321 instead of 123."

That was the answer to "For QuatView, 45° roll, 30° pitch, 0 yaw, Euler/Degrees, the quaternion
shall be 0,89 / 0,37 / 0,24 / 0,1 (rounded). Using the MPU6050 and a commun Arduino library, I do read out 49° roll, 21° pitch for 0,89 / 0,37 / 0,24 / 0,1."

[ZHomeSlice]

MPU6050 is doing the rotations in the order of 321 instead of 123."
So by changing the orientations wouldn't that affect the math

Can you access the Motion Driver 6.21 files in the TDK InvenSense Developer Page
and take a look at the rotation matrix
I am using the defaults that their example code used. (Motion Driver 6.21 Version) I'm assuming this is the same on the earlier version 2.0 of the DMP library available here. (I have not edited the 4.1 version at all.)

Snips from that Motion Driver 6.21 example code:

//File: millite_test.c
/* The sensors can be mounted onto the board in any orientation. The mounting
 * matrix seen below tells the MPL how to rotate the raw data from the
 * driver(s).
 * TODO: The following matrices refer to the configuration on internal test
 * boards at Invensense. If needed, please modify the matrices to match the
 * chip-to-body matrix for your particular set up.
 */
static struct platform_data_s gyro_pdata = {
    .orientation = { 1, 0, 0,
                     0, 1, 0,
                     0, 0, 1}
};

//File: inv_mpu_dmp_motion_driver.c
/**
 *  @brief      Push gyro and accel orientation to the DMP.
 *  The orientation is represented here as the output of
 *  @e inv_orientation_matrix_to_scalar.
 *  @param[in]  orient  Gyro and accel orientation in body frame.
 *  @return     0 if successful.
 */
int dmp_set_orientation(unsigned short orient){....

What I did is I pre-loaded this orentation into the DMP image but it can be changed after the fact to another orientation.
It' quite the chunk of code to make the change lol but it's possible.
Z

[bk4nt]

I see, I see that in TDK InvenSense package, they allow to define an orientation matrix for each chip or component, assuming they could be mounted differently than aligned and flat. Then in main.c, they do:

   /* Set chip-to-body orientation matrix. ... */

    inv_set_gyro_orientation_and_scale(....
    inv_set_accel_orientation_and_scale(...
    #ifdef COMPASS_ENABLED
    inv_set_compass_orientation_and_scale(...

I do not know if the "pre-loaded orentation into the DMP image" is the issue neither if any mod there could change something. Our MPU6050 (gyro+accel) are all flat on our breakout PCBs... Is the default hard coded matrix correct in the lib?

But I'm now almost convinced that with the current lib, some math are missing in quaternion to Euler or to ypr conversion process. Which would then mean additionnal work for CPU on any sample...

I do not know how exactly those attitude conversion fonctions could be translated to Arduino codes, to make further tests:
https://fr.mathworks.com/matlabcentral/fileexchange/47318-convertattitude-m

    SINGLE ATTITUDE EXAMPLE:
    Convert an MRP to 3-2-1 Euler Angle sequence.
    newAttitude = ConvertAttitude([.1; .2; .3], 'mrp', '321') 

Here is another example I found, in Python code, with Quaternion Rotation done before any Euler/ypr math:
https://www.thepoorengineer.com/en/quaternion/#gyroRotation

[ZHomeSlice]

So let us simplify this.
Try this code out with your Quaternion values:

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps_V6_12.h"
MPU6050 mpu;
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

void setup() {
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  // Put your Fixed Values Here:
  q.w = 0;
  q.x = 0;
  q.y = 0;
  q.z = 0;


  Serial.print("quat\t");
  Serial.print(q.w);
  Serial.print("\t");
  Serial.print(q.x);
  Serial.print("\t");
  Serial.print(q.y);
  Serial.print("\t");
  Serial.println(q.z);

  mpu.dmpGetEuler(euler, &q);
  Serial.print("euler\t");
  Serial.print(euler[0] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(euler[1] * 180 / M_PI);
  Serial.print("\t");
  Serial.println(euler[2] * 180 / M_PI);

  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Serial.print("ypr\t");
  Serial.print(ypr[0] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(ypr[1] * 180 / M_PI);
  Serial.print("\t");
  Serial.print(ypr[2] * 180 / M_PI);

}
void loop() {
}

What did you find does the math work?

[bk4nt]

To run such a math test, one would need a list of quaternion values and the corresponding expected ypr outputs.

I now believe an expert in rotation sequences and quaternion/Euler angle/ypr shall have a look at the lib internals and the bunddled examples.

So the orientation matrix is hard coded in the lib:

  static struct platform_data_s gyro_pdata = {
    .orientation = { 1, 0, 0,
                     0, 1, 0,
                     0, 0, 1}
};

I digged around Euler angles, quaternions and the related maths, Making it short, looks like the DMP algorithm uses that orientation matric data to calculate the quaternion. And that any next math ops must also be done according to that orientation - if not, next math ops will need to be done according to the DMP maths results and the quaternion output.

DMP setup is done here in the InvenSense demo, using the orientation, calling dmp_set_orientation:

    dmp_load_motion_driver_firmware();
    dmp_set_orientation(
        inv_orientation_matrix_to_scalar(gyro_pdata.orientation));

Digging further in InvenSense lib and source codes, you shall find references to xyx, xyz, xzx... associated to the orientation matrix data and functions.

A search on Google for Euler plus xyx, xyz, xzx, xzy, yxy, yxz, yzx, yzy, zxy, zxz returns some infos about the maths that shall be done according to the orientation or Euler rotations. So I found that code which lists the maths from quaternions to Euler angles "based on various rotation sequences":
http://bediyap.com/programming/convert-quaternion-to-euler-rotations/

By the way, I noticed issue #300 was opened for similar reasons:
#300 "I wanted to use euler angles as sensor inputs for PID algorithm instead of using tilt angles. I realized that euler angles are not behaving like they should be, after that I compared the equations in code and wiki. After modifying the equations as in my previous message, euler (Z-Y-X) angles seems to be correct."

So looks like the DMP setup inside the lib and the lib current maths are inconsistent. I've now done some tests skipping mpu.dmpGetEuler(euler, &q); call inside MPU6050_DMP6_using_DMP_V6.12, and this seems to confirm there are maths issues with the lib and the bundled examples, at least, case zxy (not zyx, neither xyz) is much closer to my expectations:

  // see http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
  void threeaxisrot(double r11, double r12, double r21, double r31, double r32, float res[]){
    res[0] = atan2( r31, r32 );
    res[1] = asin ( r21 );
    res[2] = atan2( r11, r12 );
  }
  
  ...
  
  //        **Skipped:**
  //            **mpu.dmpGetEuler(euler, &q);**

  // see http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
  //    **case zyx**:
      threeaxisrot( 2*(q.x*q.y + q.w*q.z),
                     q.w*q.w + q.x*q.x - q.y*q.y - q.z*q.z,
                    -2*(q.x*q.z - q.w*q.y),
                     2*(q.y*q.z + q.w*q.x),
                     q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                     euler);
            Serial.print("\teuler zyx: ");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print(" ");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print(" ");
            Serial.print(euler[2] * 180/M_PI);

  // see http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
  //    case zxy:
      threeaxisrot( -2*(q.x*q.y - q.w*q.z),
                      q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z,
                      2*(q.y*q.z + q.w*q.x),
                     -2*(q.x*q.z - q.w*q.y),
                      q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z,
                      euler);
            Serial.print("\teuler zxy: ");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print(" ");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print(" ");
            Serial.print(euler[2] * 180/M_PI);

  // see https://github.com/jrowberg/i2cdevlib/issues/300
  /* "Quaternion to euler angle conversion function "uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q)"   
   * seems to be wrong placed in "MPU6050_6Axis_MotionApps20.h" if standart Tait Bryan angles is used as
   * transformation convention (ZYX rotation). We may refer to wikipedia link below:
   * https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles"
    */
  float data[3];
  data[0] = atan2(2*(q.w*q.z + q.x*q.y), 1-2*(q.y*q.y + q.z*q.z));   // psi
  data[1] = asin(2*(q.w*q.y - q.z*q.x));    // theta
  data[2] = atan2(2*(q.w*q.x + q.y*q.z),1-2*(q.x*q.x+q.y*q.y));   // phi
            Serial.print("\teuler #300: ");
            Serial.print(data[0] * 180/M_PI);
            Serial.print(" ");
            Serial.print(data[1] * 180/M_PI);
            Serial.print(" ");
            Serial.print(data[2] * 180/M_PI);

But Euler angles aren't what I need. I still have to figure out what the maths should be for ypr (which excludes gravity). Or I may use Euler angles.

I've added here the modified MPU6050_DMP6_using_DMP_V6.12.ino sketch, it gets the quaternions, then run some alternate maths to get out Euler angles, as described above:
https://github.com/bk4nt/tests/blob/master/MPU6050/MPU6050_DMP6_using_DMP_V6.12.modded.ino

PS: Last days, I had tested also MPU9255 (because it features a magnetometer). And I had similar issues with roll&pitch. Now that I digged again into that sphere, I read issue with MPU9250 Sparkfun driver might be similar (take the quaternion, do your own additionnal math): https://forum.sparkfun.com/viewtopic.php?p=193530 Other related discussions are there: ENSTABretagneRobotics/razor_imu_9dof#36 (comment)

[ZHomeSlice]

Thank you for your persistence in discovering this issue. If you were to pick a fix for the Euler math.

What do you think about this change:

// Rotation Squences:
#define Quaternion_zyx  2*(q->x*q->y + q->w*q->z), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z, -2*(q->x*q->z - q->w*q->y), 2*(q->y*q->z + q->w*q->x), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
#define Quaternion_zyz 2*(q->y*q->z - q->w*q->x), 2*(q->x*q->z + q->w*q->y), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z, 2*(q->y*q->z + q->w*q->x), -2*(q->x*q->z - q->w*q->y)
#define Quaternion_zxy -2*(q->x*q->y - q->w*q->z), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z, 2*(q->y*q->z + q->w*q->x), -2*(q->x*q->z - q->w*q->y), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z
#define Quaternion_zxz 2*(q->x*q->z + q->w*q->y), -2*(q->y*q->z - q->w*q->x), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z, 2*(q->x*q->z - q->w*q->y), 2*(q->y*q->z + q->w*q->x)
#define Quaternion_yxz  2*(q->x*q->z + q->w*q->y), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z, -2*(q->y*q->z - q->w*q->x), 2*(q->x*q->y + q->w*q->z), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z
#define Quaternion_yxy 2*(q->x*q->y - q->w*q->z), 2*(q->y*q->z + q->w*q->x), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z, 2*(q->x*q->y + q->w*q->z), -2*(q->y*q->z - q->w*q->x)
#define Quaternion_yzx -2*(q->x*q->z - q->w*q->y), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z, 2*(q->x*q->y + q->w*q->z), -2*(q->y*q->z - q->w*q->x), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z
#define Quaternion_yzy 2*(q->y*q->z + q->w*q->x), -2*(q->x*q->y - q->w*q->z), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z, 2*(q->y*q->z - q->w*q->x), 2*(q->x*q->y + q->w*q->z)
#define Quaternion_xyz -2*(q->y*q->z - q->w*q->x), q->w*q->w - q->x*q->x - q->y*q->y + q->z*q->z, 2*(q->x*q->z + q->w*q->y), -2*(q->x*q->y - q->w*q->z), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z
#define Quaternion_xyx 2*(q->x*q->y + q->w*q->z), -2*(q->x*q->z - q->w*q->y), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z, 2*(q->x*q->y - q->w*q->z), 2*(q->x*q->z + q->w*q->y)
#define Quaternion_xzy 2*(q->y*q->z + q->w*q->x), q->w*q->w - q->x*q->x + q->y*q->y - q->z*q->z, -2*(q->x*q->y - q->w*q->z), 2*(q->x*q->z + q->w*q->y), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z
#define Quaternion_xzx 2*(q->x*q->z - q->w*q->y), 2*(q->x*q->y + q->w*q->z), q->w*q->w + q->x*q->x - q->y*q->y - q->z*q->z, 2*(q->x*q->z + q->w*q->y), -2*(q->x*q->y - q->w*q->z)
void threeaxisrot(double r11, double r12, double r21, double r31, double r32, float res[]){
	res[0] = atan2( r31, r32 );
	res[1] = asin ( r21 );
	res[2] = atan2( r11, r12 );
}

#ifdef USE_OLD_DMPGETEULER
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q){
    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
    return 0;
}
#else
uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q){
	threeaxisrot(Quaternion_zxy ,data); // Change the Sequence of  X Y Z in the equation to meet the correct Sequence
}
#endif

To use replace the dmpGetEuler function with the above code.

Would this fix the Eular side? and give every rotation option available?

Z

[bk4nt]

The conventions are standard, and very limited. By the fundamental orientation maths. There are 12 conventions for Euler, 6 conventions for YPR. I still didn't find the 6 corresponding YPR maths.

So what you suggest for Euler angles could be a fix with a warning or an error flag using pragmas: "please, pick your convention for dmpGetEuler". For YPR, what the 6 maths ops should be remains to be defined...

But I'm now trying to figure out how one could configure the DMP to force what it outputs. Might be a better choice. If the DMP would output quaternions based on eg XYZ convention, the next maths ops would be strict and unique. So far, I didn't succeed to force the DMP.

In InvenSense demo code, see around motion_driver_6.12\msp430\eMD-6.0\core. If my understanding is correct, forcing the DMP to output quaternions according to precise math convention shall be done during DMP setup phase, using the matrix (if tweaks are required), and then calling:

dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));

There also, we can see InvenSense refer to those commun math conventions:

unsigned short inv_orientation_matrix_to_scalar(   
    const signed char *mtx)   
{   
    unsigned short scalar;    
    /*  
       XYZ  010_001_000 Identity Matrix  
       XZY  001_010_000  
       YXZ  010_000_001  
       YZX  000_010_001  
       ZXY  001_000_010  
       ZYX  000_001_010  
     */   

I'm stucked in "int dmp_set_orientation(unsigned short orient)" and in "mpu_write_mem" (a double write op to regs/mem), where I don't succeed to push correctly the data to the appropriate MPU/DMP memory. If that could work, it would be a much more decent fix, with unique maths ops and the DMP forced to calculate accordingly.

// To be checked... mpu_write_mem(FCFG_1, 3, gyro_regs)
    tmp[0] = (unsigned char)(FCFG_1 >> 8);
    tmp[1] = (unsigned char)(FCFG_1 & 0xFF);
    I2Cdev::writeBytes(0x68, 0x6D, 2, tmp); // 0x6D = gyro_reg_bank_sel
    I2Cdev::writeBytes(0x68, 0x6F, 3, gyro_regs); // 0x6F = gyro_reg_mem_r_w

[ZHomeSlice]

I'm not at my home till later but this will get you direct access to the dmp memory

MPU6050 class functions to access DMP memory

setMemoryStartAddress(address);

writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);

address is the memory locating of the DMP firmware to change
Set this first using setMemoryStartAddress then change the byte using
writeBytes

chunkSize can be set to 1
and progBuffer is an array of 1 byte works to change a single byte
Or you can pass the memory location of the byte using &bar
Z

[ZHomeSlice]

Somewhat messy but...

#define KEY_CFG_25                  (0)
#define KEY_CFG_24                  (KEY_CFG_25 + 1)
#define KEY_CFG_26                  (KEY_CFG_24 + 1)
#define KEY_CFG_27                  (KEY_CFG_26 + 1)
#define KEY_CFG_21                  (KEY_CFG_27 + 1)
#define KEY_CFG_20                  (KEY_CFG_21 + 1)
#define KEY_CFG_TAP4                (KEY_CFG_20 + 1)
#define KEY_CFG_TAP5                (KEY_CFG_TAP4 + 1)
#define KEY_CFG_TAP6                (KEY_CFG_TAP5 + 1)
#define KEY_CFG_TAP7                (KEY_CFG_TAP6 + 1)
#define KEY_CFG_TAP0                (KEY_CFG_TAP7 + 1)
#define KEY_CFG_TAP1                (KEY_CFG_TAP0 + 1)
#define KEY_CFG_TAP2                (KEY_CFG_TAP1 + 1)
#define KEY_CFG_TAP3                (KEY_CFG_TAP2 + 1)
#define KEY_CFG_TAP_QUANTIZE        (KEY_CFG_TAP3 + 1)
#define KEY_CFG_TAP_JERK            (KEY_CFG_TAP_QUANTIZE + 1)
#define KEY_CFG_DR_INT              (KEY_CFG_TAP_JERK + 1)
#define KEY_CFG_AUTH                (KEY_CFG_DR_INT + 1)
#define KEY_CFG_TAP_SAVE_ACCB       (KEY_CFG_AUTH + 1)
#define KEY_CFG_TAP_CLEAR_STICKY    (KEY_CFG_TAP_SAVE_ACCB + 1)
#define KEY_CFG_FIFO_ON_EVENT       (KEY_CFG_TAP_CLEAR_STICKY + 1)
#define KEY_FCFG_ACCEL_INPUT        (KEY_CFG_FIFO_ON_EVENT + 1)
#define KEY_FCFG_ACCEL_INIT         (KEY_FCFG_ACCEL_INPUT + 1)
#define KEY_CFG_23                  (KEY_FCFG_ACCEL_INIT + 1)
#define KEY_FCFG_1                  (KEY_CFG_23 + 1)
#define KEY_FCFG_3                  (KEY_FCFG_1 + 1)
#define KEY_FCFG_2                  (KEY_FCFG_3 + 1)
#define KEY_CFG_3D                  (KEY_FCFG_2 + 1)
#define KEY_CFG_3B                  (KEY_CFG_3D + 1)
#define KEY_CFG_3C                  (KEY_CFG_3B + 1)
#define KEY_FCFG_5                  (KEY_CFG_3C + 1)
#define KEY_FCFG_4                  (KEY_FCFG_5 + 1)
#define KEY_FCFG_7                  (KEY_FCFG_4 + 1)

int mpu_write_mem(uint16_t  mem_addr,  uint8_t *Data) {
	writeWords(devAddr, 0x6D, 1, &mem_addr); //   DMP Bank Select for Loading Image
	writeBytes(devAddr, 0x6F,  1, Data); // DMP Image Loading Location
        return 0;
}

/**
 *  @brief      Push gyro and accel orientation to the DMP.
 *  The orientation is represented here as the output of
 *  @e inv_orientation_matrix_to_scalar.
 *  @param[in]  orient  Gyro and accel orientation in body frame.
 *  @return     0 if successful.
 */
int dmp_set_orientation(unsigned short orient)
{
    unsigned char gyro_regs[3], accel_regs[3];
    const unsigned char gyro_axes[3] = {0x4C, 0xCD, 0x6C};
    const unsigned char accel_axes[3] = {0x0C, 0xC9, 0x2C};
    const unsigned char gyro_sign[3] = {0x36, 0x56, 0x76};
    const unsigned char accel_sign[3] = {0x26, 0x46, 0x66};

    gyro_regs[0] = gyro_axes[orient & 3];
    gyro_regs[1] = gyro_axes[(orient >> 3) & 3];
    gyro_regs[2] = gyro_axes[(orient >> 6) & 3];
    accel_regs[0] = accel_axes[orient & 3];
    accel_regs[1] = accel_axes[(orient >> 3) & 3];
    accel_regs[2] = accel_axes[(orient >> 6) & 3];

    /* Chip-to-body, axes only. */
    if (mpu_write_mem(FCFG_1, 3, gyro_regs))
        return -1;
    if (mpu_write_mem(FCFG_2, 3, accel_regs))
        return -1;

    memcpy(gyro_regs, gyro_sign, 3);
    memcpy(accel_regs, accel_sign, 3);
    if (orient & 4) {
        gyro_regs[0] |= 1;
        accel_regs[0] |= 1;
    }
    if (orient & 0x20) {
        gyro_regs[1] |= 1;
        accel_regs[1] |= 1;
    }
    if (orient & 0x100) {
        gyro_regs[2] |= 1;
        accel_regs[2] |= 1;
    }

    /* Chip-to-body, sign only. */
    if (mpu_write_mem(FCFG_3, 3, gyro_regs))
        return -1;
    if (mpu_write_mem(FCFG_7, 3, accel_regs))
        return -1;
    dmp.orient = orient;
    return 0;
}

This should work.
now all you need to generate is the orientation value to be set into the firmware

Matches example code memory dump for V6.12 I don't think it will work with any other version.

Concerning MPU9255
this has a mpu6500 in it which is an updated version of the MPU6050 almost exactly the same except for the memory location to store offsets which I have recently handled with the latest versions of the
The DMP firmware is exactly the same for MPU6050, 6500, 9150, 9250 and mpu9255

[bk4nt]

Ok, I see, I had figured out the same from InvenSence source code, this is functionnaly almost equivalent:

int mpu_write_mem(uint16_t  mem_addr,  uint8_t *Data) {
	writeWords(devAddr, 0x6D, 1, &mem_addr); //  uint16_t  mem_addr to (uint8_t *)&mem_addr
	writeBytes(devAddr, 0x6F,  1, Data); // Push the data to selected mem_addr

    tmp[0] = (unsigned char)(FCFG_1 >> 8); // splits mem_addr
    tmp[1] = (unsigned char)(FCFG_1 & 0xFF);
    I2Cdev::writeBytes(0x68, 0x6D, 2, tmp); // 0x6D = gyro_reg_bank_sel and write there mem_addr
    I2Cdev::writeBytes(0x68, 0x6F, 3, gyro_regs); // 0x6F = gyro_reg_mem_r_w and write there data

But we need to write there "3, gyro_regs", 3 bytes. Regs data are defined as 3 bytes:

unsigned char gyro_regs[3], accel_regs[3];

I believe mpu_write_mem needs that mod, for data lenght:

int mpu_write_mem(uint16_t  mem_addr, uint8_t length,  uint8_t *Data) {
	writeWords(devAddr, 0x6D, 1, &mem_addr); //  uint16_t  mem_addr to (uint8_t *)&mem_addr
	writeBytes(devAddr, 0x6F, length, Data); // Push the data to selected mem_addr

What the outcome is confuses me. Because I'm now getting some minimal angle errors in my outputs, and because so far, DMP output convention looks to be ZXY only (but with euler angles signs changing according to the used chip orientation matrix), I have to figure out what I could have done wrong to get now some 1-2° angles errors, in init, maybe with the math ops, or with my mechanical setup.

What I'm now running is here, allows to chose a matrix or not, allows also to swap euler math methods during runtime: https://github.com/bk4nt/tests/blob/master/MPU6050/MPU6050_DMP6_using_DMP_V6.12.modded_DMP_settings.ino

Looking further, DMP may only output ZXY quaternions, with an exception for Android only (using DMP_FEATURE_ANDROID_ORIENT).

/**
 *  @brief      Enable DMP features.
 *  The following \#define's are used in the input mask:
 *  \n DMP_FEATURE_TAP
 *  \n DMP_FEATURE_ANDROID_ORIENT
 *  \n DMP_FEATURE_LP_QUAT
 *  \n DMP_FEATURE_6X_LP_QUAT
 *  \n DMP_FEATURE_GYRO_CAL
 *  \n DMP_FEATURE_SEND_RAW_ACCEL
 *  \n DMP_FEATURE_SEND_RAW_GYRO
 *  \n NOTE: DMP_FEATURE_LP_QUAT and DMP_FEATURE_6X_LP_QUAT are mutually
 *  exclusive.
 *  \n NOTE: DMP_FEATURE_SEND_RAW_GYRO and DMP_FEATURE_SEND_CAL_GYRO are also
 *  mutually exclusive.
 *  @param[in]  mask    Mask of features to enable.
 *  @return     0 if successful.
 */
int dmp_enable_feature(unsigned short mask)
{

Here is an NXP application note inside which they consider "Aerospace, Android and Windows Coordinate Systems":
https://www.nxp.com/docs/en/application-note/AN5017.pdf

We might have to figure out what ZXY convention is. Or we could experiment with DMP_FEATURE_ANDROID_ORIENT.

Edit... I'll check again later. Matrix#4 could produce ZYX quaternions outputs.

[ZHomeSlice]

Android Orient is enabled here

    /* Send gesture data to the FIFO. */
    if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT)) // if Android orient is used
        tmp[0] = DINA20;
    else
        tmp[0] = 0xD8;
    mpu_write_mem(CFG_27,1,tmp);

//...and...

    if (mask & DMP_FEATURE_ANDROID_ORIENT) {// if Android orient is used 
        tmp[0] = 0xD9;
    } else // Not used
        tmp[0] = 0xD8;
    mpu_write_mem(CFG_ANDROID_ORIENT_INT, 1, tmp);

Note that the FIFO Buffer is now increased by 4 bytes

    if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
        dmp.packet_length += 4;

To Decode the gestures used

    if (dmp.feature_mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
        decode_gesture(fifo_data + ii);

The ANDROID_ORIENT is only a small portion of the char array sent. Just 2 bits 6 & 7 which are >>6 shifted to bits 0 and 1 or just values 0-3.

/**
 *  @brief      Decode the four-byte gesture data and execute any callbacks.
 *  @param[in]  gesture Gesture data from DMP packet.
 *  @return     0 if successful.
 */
static int decode_gesture(unsigned char *gesture)
{
    unsigned char tap, android_orient;

    android_orient = gesture[3] & 0xC0;
    tap = 0x3F & gesture[3];

    if (gesture[1] & INT_SRC_TAP) {
        unsigned char direction, count;
        direction = tap >> 3;
        count = (tap % 8) + 1;
        if (dmp.tap_cb)
            dmp.tap_cb(direction, count);
    }

    if (gesture[1] & INT_SRC_ANDROID_ORIENT) {
        if (dmp.android_orient_cb)
            dmp.android_orient_cb(android_orient >> 6);
    }

    return 0;
}

I do not believe that ANDROID_ORIENT will fix this problem.
With the desire to keep things small. I didn't enable this feature in the DMP as it changes the FIFO buffer structure adding more data to the packet size and requiring additional code to handle the now larger packet.

Z