Calibrating Yaw Pitch Roll angles to zero whenever

[remoteflying]

Hello there,

I want to set Yaw, Pitch and Roll values to 0 when I decide on a MPU6050 module. I mean, calibrating the MPU6050 whenever I want. I'm using MPU6050_DMP6.ino code for testing this. I have already tried the following functions as said in the issue #486:

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

But it does not work properly as the sensor gives correct values the first milliseconds and then it gives incorrect readings. It seems like the sensor is not prepared physically to be calibrated in any position that is not horizontal. But I'm not sure about that. I tried also to use a rotation matrix to YPR angles, but it's not possible because YPR angles require a gravity vector to calculate it, as you can see in YPR function:

//Euler angles
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
//YPR angles
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

In Euler angles it is possible to calibrate them using a rotation matrix. But I need the YPR angles for my project.

So my question is if there is a way to set to 0 the YPR angles from a certain position in the space whenever I want. NOTE: I'm not an expert in programming, so I couldn't get into deep code to find a solution yet.

Thank you for your help,

[ZHomeSlice]

But it does not work properly as the sensor gives correct values the first milliseconds and then it gives incorrect readings. It seems like the sensor is not prepared physically to be calibrated in any position that is not horizontal. But I'm not sure about that

Calibration focuses on making the returned values Zero so with acceleration mpu.CalibrateAccel(6); gravity is present then one of the accelerometer readings can't be zero. I subtract gravity from the z access to achieve zero during calibration, you could subtract gravity from the accelerometer reading that is affected during the calibration process as a solution. the mpu.CalibrateGyro(6); just need the MPU to be still.

line 3274 in mpu6050.cpp if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity set i==0 or 1 for X or Y

Z

.

[remoteflying]

I think I understand you. Your solution would be ok if I rotate exactly 90º on Pitch or Roll. But my issue is that I need to calibrate the sensor when it is in any orientation, not just aligning X, Y or Z axis to gravity vector. I'm right? Also, I would like not to say to the code in what position is the module when I calibrate it. I may put the horizontal offsets and then calibrate the sensor using this offsets as a reference. If it is possible, fine. If not, I can deal with it.

[ZHomeSlice]

I would set and fix your acceleration offsets by using the following lines. then with good offsets you shouldn't need to calibrate every time.

    mpu.setXAccelOffset(1150); 
    mpu.setYAccelOffset(-50); 
    mpu.setZAccelOffset(1060);

How do I get the numbers Set your MPU on a level surface with the correct orientation. the extra loops will refine the accuracy of the results each loop tightens the PID settings. 20 is overkill but you can't go wrong lol

mpu.CalibrateAccel(20);

then get the results:

        Serial.println();
        mpu.PrintActiveOffsets();

replace the incorrect offsets with the newly generated ones supplied through calibration. you can do this with the gyro but yaw is affected by temperature and can start drifting even after calibration over time. recalibrating this value may be necessary as the gyro calibration just needs to have the mpu still during the calibration process. it is looking for no change in the rotation rather than having gravity to deal with.

also, note that I have enabled a sweet feature in the V6.12 Firmware as it will automatically calibrate the gyro after 8 seconds of no motion.

How calibration behaves: The calibration routine takes the current offsets and uses them to start fine-tuning the results repeating the calibration over and over will only make minor tweaks to the results as it gets the current offsets to use as a starting point for further calibration.

Z

[remoteflying]

I tried to calibrate the sensor when it is tilted 90º in Pitch and I replaced the Offset values form the setup(). I visualize the same errors as I said before:

But it does not work properly as the sensor gives correct values the first milliseconds and then it gives incorrect readings. It seems like the sensor is not prepared physically to be calibrated in any position that is not horizontal. But I'm not sure about that.

To show you the problem I created a code starting from Horizontal Offsets and then, when I enter a character in the serial it changes to the Offsets previously calibrated. Serial output says:

[...]
Calibrated when is Horizontal   ypr -0.11   0.16    0.24        
Calibrated when is Horizontal   ypr -0.11   0.16    0.24        
Calibrated when is Horizontal   ypr -0.11   0.16    0.24    
//Here I changed the offsets
Calibrated when is 90º Pitch    ypr -0.11   0.16    0.24        
Calibrated when is 90º Pitch    ypr -0.11   0.04    0.24        
Calibrated when is 90º Pitch    ypr -0.10   -0.36   0.24        
Calibrated when is 90º Pitch    ypr -0.10   -0.76   0.23        
Calibrated when is 90º Pitch    ypr -0.10   -1.17   0.22        
Calibrated when is 90º Pitch    ypr -0.10   -1.56   0.22        
Calibrated when is 90º Pitch    ypr -0.10   -1.94   0.21        
Calibrated when is 90º Pitch    ypr -0.09   -2.33   0.20        
//Pitch continue reducing... and after 4 seconds it shows:
Calibrated when is 90º Pitch    ypr -0.17   -24.81  -0.18       
Calibrated when is 90º Pitch    ypr -0.17   -24.81  -0.18       
Calibrated when is 90º Pitch    ypr -0.17   -24.81  -0.18       
[...]

As you see the Pitch do not reach -90º as it should. If I tilt the sensor to -90º the Pitch says values near 0 after 4 seconds also. If I turn it 90º quickly to horizontal position the pitch shows me correct values but just the first milliseconds as I said before, and then it go again to -24.81, after 4 seconds...

With horizontal offsets the sensor works fine, but with different offsets it doesn't. I hope the sensor has not physical limits to do what I want it to do.

[ZHomeSlice]

Consider showing the raw values of acceleration instead of the Quaternion results. the Calibration may be holding but the Quaternion calculations may be in error.

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); this will get the raw rate values. they will update @ every 1ms.

These values should resolve to near Zero (level) or 1G (16384 for Vertical) after calibration and remain there. Calibration and the offsets it generates is nothing more than making these values hold as close to zero as possible.

I used a PID loop to achieve this as the former averaging these readings take forever and still doesn't give an accurate offset.

If these values hold near zero then we are probably not using Quaternions correctly or some other math is affecting our results.

Z

[remoteflying]

I modify the code from MPU6050_raw.ino with the two different Offsets as the previous example:

Calibrated when is Horizontal   a/g:    -70 178 16144   -17 -18 -4
Calibrated when is Horizontal   a/g:    -70 178 16144   -17 -18 -4
Calibrated when is Horizontal   a/g:    -130    158 16148   -19 -12 2
Calibrated when is Horizontal   a/g:    -156    66  16074   -14 -17 3
Calibrated when is +90 Pitch    a/g:    -156    66  16074   -14 -17 3
Calibrated when is +90 Pitch    a/g:    -9292   232 23792   7   -46 8
Calibrated when is +90 Pitch    a/g:    -9292   232 23792   7   -46 8
Calibrated when is +90 Pitch    a/g:    -13816  110 30426   -185    294 -14
Calibrated when is +90 Pitch    a/g:    -15060  -306    32740   -679    584 31
Calibrated when is +90 Pitch    a/g:    -15060  -306    32740   -679    584 31
Calibrated when is +90 Pitch    a/g:    -15540  -166    32767   -170    353 1
Calibrated when is +90 Pitch    a/g:    -15220  68  32476   126 -145    -39
Calibrated when is +90 Pitch    a/g:    -15220  68  32476   126 -145    -39

The transition is superfast fast in Raw values. Just a few milliseconds.

[ZHomeSlice]

That is interesting. Let's Trigger the calibration a second time after the deviation settles down. And let's see the results. Again.

Calibration focuses on forcing those values to be as close to zero as possible. Calibration uses the last calibration or recently set offsets as a starting point. So running it over and over again is possible and will correct for changes. The desired offsets should be set into the code for future startups even when calibrating. Z

[remoteflying]

Now I show you the code just to make sure that we are talking about the same. This fragment is inside the main loop:

//If serial reads something, then change offsets
        if(Serial.available() && Serial.read()){ // empty buffer
          while (!Serial.available());                 // wait for data
          while (Serial.available() && Serial.read()); // empty buffer again
            if(isP90Pitch==0){ //If sensor is horizontal set 90º Pitch offsets
mpu.setXAccelOffset(-4992.71);
mpu.setYAccelOffset(-1836.17);
mpu.setZAccelOffset(3374.33);
mpu.setXGyroOffset(58.50);
mpu.setYGyroOffset(-44.25);
mpu.setZGyroOffset(-3.00);
          }
          else{                //If sensor is tilt 90º Pitch set horizontal offsets
mpu.setXAccelOffset(-3193.17);
mpu.setYAccelOffset(-1825.50);
mpu.setZAccelOffset(1426.75);
mpu.setXGyroOffset(55.25);
mpu.setYGyroOffset(-44.25);
mpu.setZGyroOffset(-3.00);          
          }
            isP90Pitch=!isP90Pitch;  //Change the tilt state
        }
        //Serial print the state
            Serial.print("Calibrated when is ");
        if(isP90Pitch==0){Serial.print("Horizontal\t");}
        else {Serial.print("+90 Pitch \t");}

Ok. Do you suggest calling the following functions repeatedly after that 4 seconds on deviation?

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

Or do you suggest to set again the offsets repeatedly after deviation is settle down?

[ZHomeSlice]

line 3274 in mpu6050.cpp
if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 16384; //remove Gravity from X

if ((ReadAddress == 0x3B)&&(i == 1)) Reading -= 16384; //remove Gravity from Y

if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity from Z

// Also, Consider inverted readings -1g or adding 16384 

if ((ReadAddress == 0x3B)&&(i == 0)) Reading += 16384; //remove Gravity from X

if ((ReadAddress == 0x3B)&&(i == 1)) Reading += 16384; //remove Gravity from Y

if ((ReadAddress == 0x3B)&&(i == 2)) Reading += 16384; //remove Gravity from Z

Modify the mpu6050.cpp function CalibrateAccel(); to handle offsets in the orientation you're expecting to use

Then get the new offsets

mpu.PrintActiveOffsets();

while sitting still you start to see deviations off of Zero

use mpu.CalibrateAccel(20);

the extra loops will fine-tune it, even more, they should execute extremely fast as the only use about 100 readings with the tighter and tighter PID settings for every step.

Then get the new offsets

mpu.PrintActiveOffsets();

The offsets shouldn't be much different from earlier calibrations

Calibrated when is Horizontal a/g: -70 178 16144 -17 -18 -4 1G Gravity is 16384

Something is wrong as we don't see gravity move to another axis

Calibrated when is +90 Pitch a/g: -15220 68 32476 126 -145 -39 -15220 is close to -1g 32476 is closer to 2G Definatly not calibrated

Possible Test: To get an idea where your offsets should be force the acceleration offsets to Zero

mpu.setXAccelOffset(0);
mpu.setYAccelOffset(0);
mpu.setZAccelOffset(0);

then get some raw readings Subtract gravity or add it if it shows up as a negative to get close to zero

then divide the values by 8, this should somewhat represent your target offsets. Dividing by 8 shifts the int to the right removing some precision

mpu.setXAccelOffset( round((reading +- Gravity) /8));  // the number you come up with should be an int closer to zero by gravity or 16384 
mpu.setYAccelOffset(round(reading /8)); // deviding by 8 shifts the bitmap to the right by 3 (reading >>3)
mpu.setZAccelOffset( round(reading/8)); // deviding by 8 shifts the bitmap to the right by 3 (reading >>3)

The readings with these test offsets should be +-500 from zero remember that 16144 is just 1G so +- 500 isnt much to clean up.

Z

p.s. floats are not good here these should be INTs mpu.setXAccelOffset(-3193.17); mpu.setYAccelOffset(-1825.50); mpu.setZAccelOffset(1426.75);

[remoteflying]

Modify the mpu6050.cpp function CalibrateAccel(); to handle offsets in the orientation you're expecting to use

But changing that lines will give me offsets just in +-90º or 180º orientation of Pitch or Roll. Doesn't it? Remember I asked in first Comment:

So my question is if there is a way to set to 0 the YPR angles from a certain position in the space whenever I want.

I have put 90º Pitch just for testing, but I will need orientations like: ypr -9.10 -52.36 130.24 Or whatever position in the space to be calibrated as Offsets. So will I be capable to reach my goal?

Thank you for your effort.

[ZHomeSlice]

So how the MPU works. when acceleration is detected the MPU sends a rate. The rate is limited to a 16-bit number between −32,768 to 32,767 and 1G of gravity is 16,384 or 1/4 the full range of the Integer The MPU range is +-2G Since Gravity is always present here on earth in order to calibrate the accelerometer you must remove it by subtracting it from your readings to calibrate it. If you absolutely know where gravity is when you start your calibration you can subtract the portion of gravity from each of your readings to achieve this.

line 3274 in mpu6050.cpp
if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 16384; //remove Gravity from X
if ((ReadAddress == 0x3B)&&(i == 1)) Reading -= 16384; //remove Gravity from Y
if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity from Z

// Also, Consider inverted readings -1g or adding 16384 

if ((ReadAddress == 0x3B)&&(i == 0)) Reading += 16384; //remove Gravity from X
if ((ReadAddress == 0x3B)&&(i == 1)) Reading += 16384; //remove Gravity from Y
if ((ReadAddress == 0x3B)&&(i == 2)) Reading += 16384; //remove Gravity from Z

Hypothetical situation if you know that X is influenced by 300 and Y is influenced by 1600 and z is influenced by 84 you just subtract them from the calibration and your set.

Now if your MPU is at a random position then your guess is as good as mine and I have no way of telling you how to discover gravities pull on you MPU. Z

[remoteflying]

You said:

Modify the mpu6050.cpp function CalibrateAccel(); to handle offsets in the orientation you're expecting to use

void MPU6050::CalibrateAccel(uint8_t Loops ) {

    float kP = 0.3;
    float kI = 20;
    float x;
    x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
    kP *= x;
    kI *= x;
    PID( 0x3B, kP, kI,  Loops);
}

Should I modify the kP and kI of PID functions? I guess you are proposing to change lines 3274 in mpu6050.cpp (as you said several times). So what I've done is:

1. I tilt the sensor to one of the positions I want to calibrate the sensor.
2. I Set AccelOffsets to 0.

   mpu.setXAccelOffset(0);
   mpu.setYAccelOffset(0);
   mpu.setZAccelOffset(0);
   mpu.setXGyroOffset(55);
   mpu.setYGyroOffset(-44);
   mpu.setZGyroOffset(-3);

   Serial shows me this now:

   a/g:    32767   15682   1822    3   -12 -1
   a/g:    32767   15688   1852    -1  -13 1
   a/g:    32767   15666   1830    -9  -8  -7

3. Then I modify lines 3274 in mpu6050.cpp as this:

   if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 32767; //remove Gravity from X
   if ((ReadAddress == 0x3B)&&(i == 1)) Reading -= 15670 //remove Gravity from Y
   if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 1835; //remove Gravity from Z

   ...to remove gravity from each axis.

NOTE: In this case X reach his limit of 32767, so I will probably need to subtract more to reach 0. Then I get this results:

a/g:    32767   15748   1728    -3  -5  -4
a/g:    32767   15714   1756    -3  -3  -1
a/g:    32767   15726   1792    0   -6  -8

Yes, the same results. I verify that I'm using the correct mpu6050.cpp library. It seems that the function mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); of MPU6050_raw.ino does not go into lines 3274 in mpu6050.cpp.

4. I try to do the same for MPU6050_DMP6.ino and It doesn't change at all when I modify that lines.

Now if your MPU is at a random position then your guess is as good as mine and I have no way of telling you how to discover gravities pull on you MPU.

It's not that case. I need to calibrate the sensor in some positions and store the calibrations to use them in the project code whenever I want. However, it would be better if I could calibrate the sensor regardless of its position. First we can start for some certain positions.

[ZHomeSlice]

I'm not 100% sure but logic would imply that the sum of all the gravity values removed would equal 16,384 1G 32767 + 15670 + 1835 =50272 or just over 3G of correction. you are only removing the influence gravity has on each axis

also, note that offsets are not equal to the readings they are smaller by I believe 1/4 but I will need to review my code to be sure. in other words, changing offsets by 1 will have an influence of 4 on the output. I will update this tomorrow if I'm incorrect in the divisor.

Z

[remoteflying]

I'm not 100% sure but logic would imply that the sum of all the gravity values removed would equal 16,384 1G

It's more complex than the sum of gravity values, I think. I get, for example: Calibrated when is Horizontal a/g: 900 -7162 14734 and Calibrated when is Horizontal a/g: 3156 -10614 11484 Then the opperation or function should be like this:

Well I tried to change that lines f mpu6050.cpp and now I think I understand more how the code works. I get to these points:

-> If you want to calibrate the sensor regardless of its position (and just that) you must introduce the raw values in lines 3274 in mpu6050.cpp from the raw readings of a tilted sensor calibrated in horizontal position. For example, I tilted the sensor 30º Roll aprox. (see experiment picture).

From this: Calibrated when is Horizontal a/g: 6 7248 14050 -2 4 -2 ypr 13.81 3.31 28.34 To this:

if ((ReadAddress == 0x3B)&&(i == 0)) Reading -= 6;      //remove Gravity from X
if ((ReadAddress == 0x3B)&&(i == 1)) Reading -= 7248;   //remove Gravity from Y
if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 14050;  //remove Gravity 

I tested it by rotating very fast the Yaw and as you can see only Yaw is being affected, neither Roll nor Pitch. Just Yaw.

a/g:    -518    7684    14188   -18 -44 -93 ypr 18.61   -1.05   28.08   
a/g:    -394    7662    14148   -11 -33 -83 ypr 18.66   -1.06   28.08   
a/g:    -356    7462    14142   -4  -20 -87 ypr 18.72   -1.08   28.08   
a/g:    -726    7406    14378   2   -35 -102    ypr 18.79   -1.09   28.07   
a/g:    -754    7468    14364   -20 -78 -147    ypr 18.90   -1.11   28.05   
a/g:    -830    7280    14840   -30 -137    -172    ypr 19.02   -1.08   28.01   
a/g:    -11650  -2366   19932   -129    -356    -509    ypr 19.72   -1.21   27.69   
a/g:    -10782  -228    20250   -173    -1239   -1838   ypr 21.24   -1.35   27.29   
a/g:    -10708  1498    18748   -206    -1866   -2867   ypr 23.36   -1.40   26.89   
a/g:    -9314   1280    17302   -245    -2247   -3665   ypr 25.98   -1.47   26.47   
a/g:    -6056   3058    14452   -304    -2330   -4271   ypr 28.90   -1.58   26.07   
a/g:    -3828   4846    12212   -415    -2122   -4715   ypr 32.10   -1.82   25.67   
a/g:    1134    8080    14834   -187    -2179   -5163   ypr 35.50   -1.95   25.53   
a/g:    2182    8694    16874   -117    -2771   -5704   ypr 39.40   -1.78   25.25   
a/g:    354 8714    14860   -523    -3480   -6480   ypr 43.93   -1.49   24.82   
a/g:    52  8512    13130   -573    -3866   -7091   ypr 48.93   -1.11   24.41   

And then when it finished moving: a/g: -594 7646 14200 -56 257 501 ypr 119.67 2.48 24.49

So this is a correct calibration for that propose. But this is not what I'm looking for xddd

-> I want that the sensor shows me ypr 0 0 0 when I calibrate it.

The most aproximate test I made is this:

1. I tried to leave the mpu6050.cpp with the normal configuration: if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384; //remove Gravity from Z
2. I tilted the sensor to 30º Roll.
3. I used mpu.CalibrateAccel(20); to calibrate it. I got: a/g: 44 182 16322 -5 0 4 ypr 12.56 -0.38 0.68 It's fine. Pitch and Roll are near 0. Hurra! But then:
4. I made the same experiment. (see picture). I rotate very fast Yaw and I got this results:

a/g:    44  182 16322   -5  0   4   ypr 12.56   -0.38   0.68    
a/g:    12  152 16234   -7  -4  3   ypr 12.56   -0.37   0.67    
a/g:    114 184 16324   -8  -13 -5  ypr 12.56   -0.35   0.66    
a/g:    -70 148 16618   -22 -52 -28 ypr 12.59   -0.30   0.64    
a/g:    -204    -452    17076   -70 -163    -95 ypr 12.71   -0.18   0.56    
a/g:    -19426  -8956   20868   -295    -683    -1248   ypr 13.82   0.02    0.16    
a/g:    -20420  -10134  25062   -364    -2093   -3451   ypr 16.24   0.93    -0.32   
a/g:    -19200  -15252  28192   -636    -3285   -5396   ypr 19.87   2.64    -1.01   
a/g:    -22956  -17330  27476   -860    -4702   -7557   ypr 24.93   5.24    -1.65   
a/g:    -16860  -5934   26418   -861    -6270   -9992   ypr 31.47   8.83    -1.70   
a/g:    -12840  2884    19976   -1136   -7403   -11637  ypr 39.15   13.16   -1.06   
a/g:    -2134   6466    9058    -1688   -8039   -12306  ypr 47.50   17.93   0.15    
a/g:    -17256  7566    17860   -1865   -8121   -12833  ypr 56.61   22.29   2.00    
a/g:    -15830  8672    15476   -3081   -8444   -14061  ypr 66.66   26.37   4.36    
a/g:    -15582  15804   10248   -2904   -8390   -14055  ypr 76.51   29.94   7.77    
a/g:    -12978  19518   4450    -2856   -7632   -12675  ypr 85.46   32.70   11.63   
a/g:    -6900   28132   3080    -2507   -6522   -11068  ypr 93.18   34.49   15.68   

5. As you can see, Yaw moves correctly (apparently), but Pitch and Roll are not near 0 when I move the sensor in Yaw. After some seconds, Pitch and Roll returns to near 0.

[ZHomeSlice]

Calibration doesn't change the YPR values it only changes rates which are used to calculate YPR values in other words a level MPU gravity should be zero on the X and Y accelerometers and should show no rate of acceleration when still but when the MPU is not calibrated the rates are not close to zero so offsets must be added to achieve the proper rates in order to calculate pitch and roll correctly. Yaw is based on the Gyro mainly and since it has no reference it must be calculated or it will drift this is also a rate of change and nor a position. If the MPU has good calibration values it shouldn't drift and angles should be calculated correctly. since the yaw is calculated within the DMP by using rate over time the easiest way to remove this or reset is to subtract the current position to achieve zero then let the DMP continue to adjust for offsets. I used this for my YAW calculation allowing a shift to control a robot using PID and a desired direction "YawOffset".

  YawInput = YawMPU + YawOffset;
      if (YawInput > 180) YawInput -= 360; // rollover to negative
      if (YawInput < -180) YawInput += 360; // rollover to positive

Something like this could work for pitch and roll although I never wrote code to do such.

Z

[remoteflying]

I already create that Yaw Offset before but I'm blocked trying to create that Pitch and Roll Offset that are you saying. This is the key of the topic. Do you know how to do this? It's more complex than adding or subtracting angles on Pitch and Roll values. If you do that, you are just affecting the final output. For example if you subtract -30º to Roll, then all values will be 0 at origin position, but you will have incorrect readings as you move away from that origin orientation because the output axis will not be aligned with the sensor axis. To reach a proper solution, the global coordinate system has to be changed and the MPU has to read values from the new coordinate system. Did I explain correctly? So, do you think it is possible?

[jrowberg]

@remoteflying It sounds like you need to convert whatever the current quaternion is into a different reference frame. Presumably you would just pick whatever the current values are at your new "zero" orientation, then use that as a constant rotation quat to be applied to all incoming data. You'd have to perform this multiplication at the app level, probably:

https://stackoverflow.com/questions/18818102/convert-quaternion-representing-rotation-from-one-coordinate-system-to-another

It is possible that the DMP code allows you to specify your own "reorientation" quat so it does the heavy lifting for you, but I don't know how to do it if so. (My actual working knowledge of DMP usage is, and always has been, rather low.)

[remoteflying]

Thanks @jrowberg. In my case I need rotations different of 90º, so I need to study how to apply rotation matrix to quaternion angles.

You'd have to perform this multiplication at the app level, probably

Do you mean changing MPU6050_6Axis_MotionApps20.h file? Maybe I can do this in the main code MPU6050_DMP6.ino. Could I do something like this?

float qw0,qx0,qy0,qz0;

void calibrateCoordinates(void) {
mpu.dmpGetQuaternion(&q, fifoBuffer);
qw0=q.w;
qx0=q.x;
qy0=q.y;
qz0=q.z;
}

void quaternionRotation(Quaternion *q, float qw0, float qx0, float qy0, float qx0){
qNew= [...]
}

void loop() {
[...]

if (button1==true) calibrateCoordinates(); //change global coordinates whenever you want

mpu.dmpGetQuaternion(&q, fifoBuffer);
    quaternionRotation(&q,qw0,qx0,qy0,qz0); //rotation matrix function
mpu.dmpGetGravity(&gravity, &qNew);
mpu.dmpGetYawPitchRoll(ypr, &qNew, &gravity);

[...]
}

Even if this code is not possible, it shows chiefly what I want to archive.

[jrowberg]

Yes, that's the right idea. The .h file would probably not need to change, unless there actually is a way to make the DMP do the work for you. But in that case, it would probably require changes to more than just the .h file anyway. Getting it to work in the .ino file with code such as you supplied there is the easiest way.

The only caveat is that if you're doing a quaternion multiplication on a slow MCU (e.g. 8 MHz AVR), it's going to chew through a lot of CPU time and probably throw off the sample rate. I expect you shouldn't have a problem on a Cortex M0 like the ATSAMD21 though.

[remoteflying]

Hello again.

I have a very close solution. In short:

1. I take q Quaternion and I store the orientation I want for calibrating in p quaternion. mpu.dmpGetQuaternion(&q, fifoBuffer);
2. I convert quaternion to euler mpu.dmpGetEuler(euler, &q); mpu.dmpGetEuler(eulerP, &p);
3. Then I remove p euler angles from q angles and store the new orientation in eulerT eulerT[0]=(euler[0] - eulerP[0]); eulerT[1]=(euler[1] - eulerP[1]); eulerT[2]=(euler[2] - eulerP[2]);
4. I convert again the modified quaternion in t. t.eulerToQuaternion(eulerT[0], eulerT[1], eulerT[2]);

My problem now is f*cking gimbal lock 🤣 So what I need is very easy I think because it's related about how the MPU works. I need to reset the orientation on q when I want. It always happens that when I reset the Arduino q.z is 0, meaning that it has no rotation on Z axis. But I don't want to reset it every time.

I just need that, a magic function that when I call it, q quaternion resets.

[jrowberg]

Hi @remoteflying,

Your solution is conceptually logical, but not a good idea in practice. You're doing the following:

1. Convert from gimbal-lock-impervious format A to gimbal-lock-susceptible format B
2. Apply rotation, suffer from gimbal lock in some cases
3. Convert from gimbal-lock-susceptible format B back to gimbal-lock-impervious format A

You can apply a rotation directly within quaternion space. That's one reason they are so useful. Quaternion rotation is accomplished by multiplying one by another, with the following math:

quat qmult(quat q1, quat q2) {
    quat q3;
    q3.w = -q1.x * q2.x - q1.y * q2.y - q1.z * q2.z + q1.w * q2.w;
    q3.x =  q1.x * q2.w + q1.y * q2.z - q1.z * q2.y + q1.w * q2.x;
    q3.y = -q1.x * q2.z + q1.y * q2.w + q1.z * q2.x + q1.w * q2.y;
    q3.z =  q1.x * q2.y - q1.y * q2.x + q1.z * q2.w + q1.w * q2.z;
    return q3;
}

...where the quat data type is a struct of double-type w/x/y/z variables, representing the scalar and vector components of a quaternion, respectively.

Note that quat multiplication is NOT commutative, so Q1 * Q2 is very different from Q2 * Q1. I believe what you would want to do in this case is qDest = qmult(qRef, qLive) where qRef is your stored zero orientation and qLive is whatever the DMP is spitting out.

It is possible that the above math will actually rotate in the wrong direction though, basically doubling the "zero" orientation components instead of actual zeroing them. If this is the case, you probably need the conjugate first:

quat qconj(quat q1) {
    quat q2;
    q2.w = q1.w;
    q2.x = -q1.x;
    q2.y = -q1.y;
    q2.z = -q1.z;
    return q2;
}

It's been a while since I dug into this, so hopefully the above works.

[remoteflying]

Many many thanks, it works.

I used the library helper_3dmath.h this to calculate the final quaternion:

      mpu.dmpGetQuaternion(&q, fifoBuffer);
      t=p.getProduct(q.getConjugate()).getConjugate();

Where q is DMP output, p is the stored quaternion to rotate and t is your qDest. It's weird that I have to enter .getConjugated() twice. But it works perfect like this.

For YPR it's like this:

      mpu.dmpGetGravity(&s, &t);
      mpu.dmpGetYawPitchRoll(yprT, &q, &s);

s is a Vectorfloat, like gravity.

OMG it's quite simple. It must be simple. I worked on this for two months trying to understand quaternions and Euler angles to figure out how to join two quaternions rotations. jrowberg, you saved me much time and headache (and also code loop cycle time). Thanks :D

I still need to calibrate the Yaw to 0 every time I need. I know that in YPR mode I just have to remove the set Yaw angle from output Yaw. But just to learn a little more from the library, is there any cleaner way to do the same effect? I already ask in my comment above:

I need to reset the orientation on q when I want. It always happens that when I reset the Arduino q.z is 0, meaning that it has no rotation on Z axis. But I don't want to reset it every time.

Finally, the main problem of the topic has been solved. I'll close it once you respond.

[jrowberg]

I'm very glad to hear that it's working. Yeah, taking the conjugate twice during the process is one of those weird features of quaternion operations...I only dip back into the math from time to time, and I never seem to soak it all in since I only mess with it long enough to address the requirement of the hour. Sorry you had to figure it out on your own, but kudos. :)

[telephasicworkshop]

Thank you all for this very useful discussion! (and library!!)

I'll take the opportunity since it's not closed yet to hopefully learn more about the full solution that was used here.

I have a similar case I believe. I don't need to zero on-the-fly, but I do need to zero when the sensor is in an arbitrary orientation.

I was also able to use this quaternion product method to change the reference orientation. In my case, sensor +X is nominally "up", so as a test I tried with success to force p as: Quaternion p = {0.7071, 0, -0.7071, 0}; (I found the quaternion output to be about that when it was oriented that way)

But so far I have only had success running

mpu.CalibrateAccel(6);

with sensor +Z pointing up.

I am wondering @remoteflying how you have handled calibration and determining p whenever you want to. Do you treat finding this new reference frame like an extension to calibration (at rest), or zeroing while it is in motion? Maybe setting p to be a snapshot of dmpGetQuaternion when in the orientation you want to be the zero? In my case, the sensor could be at rest while finding the new reference quaternion and/or mpu offsets. Was it necessary in the end to do some more customized gravity subtraction as you discussed early on with @ZHomeSlice?

It seems lots of other people struggle with these sensors in different base orientations. Some tried to substitute one existing x,y,z variable for another in the various functions... a couple even reported success, though I couldn't make it work. I'm convinced that transformations like this on/just before the final output only (and not on the guts of dmp calculations) are the way to go. After your work to figure this out maybe it's worth documenting a common case like sensor upside down or vertical in the examples.