Trouble changing Pitch and Roll Offsets on MPU6050

[RemoteFlying]

Hi everyone,

I already started searching for a solution in this topic of GitHub: Calibrating Yaw Pitch Roll angles to zero whenever

I will explain again.

-> I need to calibrate the MPU6050 sensor regardless of its orientation.

First, see the image I attach.

The MPU sensor is tilted 30º Roll approx and it's calibrated horizontally. Using the code MPU6050_DMP6.ino code from i2cdevlib library, it shows me this (I show also raw values):

a/g: 0 7044 14638 5 4 -3 ypr 44.41 -0.05 25.60

If I rotate Yaw in gravity axis Yaw increases/decreases and Roll/Pitch don't move.

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

This is correct because just Yaw is affected. But what I need for this experiment is that the serial shows me:

a/g: 0 0 16384 0 0 0 ypr 0 0 0

...or near 0. So I use mpu.CalibrateAccel(20); to force calibration in that position. Then it shows me this:

a/g: 4 20 16382 4 3 -1 ypr -16.72 -0.06 -0.01

Perfect! I can subtract o add a Yaw offset, it's simple. BUT I can't do the same with Pitch and Roll because adding or subtracting offset angles will NOT change coordinate system. It will generate conflicts when obtaining values as it move away from the calibration origin.

To see this problem, let's repeat the experiment with the new calibration. I move the object quickly the same way as before. What I expect:

ypr 0 0 0
ypr 3 0 0
ypr 9 0 0
ypr 15 0 0
ypr 21 0 0
ypr 32 0 0
ypr 37 0 0

Reality:

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

As you see, Yaw changes apparently correctly, but Pitch and Roll are not still. After some seconds they go to 0 again.

So, I need to change somehow the origin orientation or coordinate system orientation and prepare the MPU to read values from this orientation.

Do you think this (or my main objective) is possible?

[jremington]

The MPU6050 cannot measure absolute yaw, because it has no North reference. If you are interested in measuring just pitch and roll, with the sensor more or less stationary during measurement, you have taken the wrong approach.

This minimal code will work fine, but for the highest accuracy, carefully calibrate the accelerometer according to this tutorial first.

// minimal MPU-6050 tilt and roll (sjr)
// works perfectly with GY-521, pitch and roll signs agree with arrows on sensor module 7/2019
// tested with eBay Pro Mini, **no external pullups on SDA and SCL** (works with internal pullups!)
//
#include<Wire.h>
const int MPU_addr1 = 0x68;
float xa, ya, za, roll, pitch;

void setup() {

  Wire.begin();                                      //begin the wire communication
  Wire.beginTransmission(MPU_addr1);                 //begin, send the slave adress (in this case 68)
  Wire.write(0x6B);                                  //make the reset (place a 0 into the 6B register)
  Wire.write(0);
  Wire.endTransmission(true);                        //end the transmission
  Serial.begin(9600);
}

void loop() {

  Wire.beginTransmission(MPU_addr1);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(MPU_addr1, 6, true); //get six bytes accelerometer data

  xa = Wire.read() << 8 | Wire.read();
  ya = Wire.read() << 8 | Wire.read();
  za = Wire.read() << 8 | Wire.read();
// formula from https://wiki.dfrobot.com/How_to_Use_a_Three-Axis_Accelerometer_for_Tilt_Sensing
  roll = atan2(ya , za) * 180.0 / PI;
  pitch = atan2(-xa , sqrt(ya * ya + za * za)) * 180.0 / PI; //take into account roll previously applied

  Serial.print("roll = ");
  Serial.print(roll,1);
  Serial.print(", pitch = ");
  Serial.println(pitch,1);
  delay(400);
}

Note that yaw, pitch and roll angles interact, because the order in which the rotations are applied matters. You cannot simply subtract an offset from both angles, and expect that to work.

Finally, because rotation angles are not independent, to determine the rotation angles that convert from one standard orientation to another requires matrix or quaternion algebra. Introduction to rotation algebra here.

[RemoteFlying]

Thanks for reply jremington,

In my project I do fast movements of the sensor so gravity vector is needed in order to calculate ypr angles. That's why I'm using MPU6050_DMP6.ino from i2cdevlib library.

To calculate YPR angles the MPU6050_DMP6.ino says:

mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

I could get an orientation offset in quaternion from my desired position and then modify the quaternion input values using a rotation. And then the ypr values will be calculated with this rotation applied. Will this work? I show you now a bit of what I'm proposing. The code might be like this (note that I don't have an idea of working with pointers).

float qw0,qx0,qy0,qz0;	//quaternion offsets
Quaternion qNew; 			//quaternion after rotation

void getQuaternionOffsets(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() {
[...]

 //Change global coordinates whenever you want
	if (button1==true) getQuaternionOffsets();

//Obtain quaternions and apply rotation from quaternion offsets
	mpu.dmpGetQuaternion(&q, fifoBuffer);
	quaternionRotation(&q, q.w, q.x, q.y, q.z);

//Get YPR values
	mpu.dmpGetGravity(&gravity, &qNew);
	mpu.dmpGetYawPitchRoll(ypr, &qNew, &gravity);

[...]
}

I spent all afternoon trying to understand quaternions and although they are beautiful and I enjoy learning them I don't know how to apply a rotation yet.

Let's say that I store a quaternion offset wxyz(0.97, 0.24, 0, 0) (which actually is a 30º roll rotation) and I have to subtract/add that angle (or rotation matrix or whatever) from the &q. So when I press button1 the &qNew memory pointer should say wxyz(1, 0, 0, 0) because it has just been calibrated and it changed the reference.

That's somehow I want to archive. So I would like to know what type of math operation I should use to apply the rotation. Is it an addition, multiplication, matrix rotation? Should I transform to Euler first?

I find this library but no examples are explained then I don't know how to use it.

[jremington]

Yes you have the basic idea. The operations can be performed using rotation matrix or quaternion algebra, but the latter is becoming quite popular.

There are several web sites with tutorials that explain how to rotate or transform quaternions, starting from the basics. Google "quaternion transformations" for example.

Don't bother with Euler angles until you need them to interpret the result.

Note that with the MPU6050, yaw has no origin and is always relative. So you can't necessarily repeat 3D transformations.

[RemoteFlying]

I don't get the formula yet. I tried with quaternion algebra because it's easy to operate. See the attached image to follow my explications.

According to what I have learned of quaternions in the last week (or month), the formula t = s · h · s' is not applicable in my case because you take a point or vector h and rotate it from s axis. In my case I don't have a point or vector, I have a quaternion vector to be rotated.

The formula t = h - s is kind of what I want to archive, as you can see in last image, but you can't subtract or add values in that way. You could do it with angles on Euler form, but not for Quaternions because the wxyz are a unit vector values, and they follow this rule: w^2+x^2+y^2+z^2=1

So, now I'm trying to find the formula for changing the axis coordinates to a given quaternion vector.

Maybe I would need to post this on a math forum, do you suggest one?

See the image in full resolution

[jremington]

The quaternion that represents the body orientation IS a rotation operator, applied to one axial frame to produce the other. Rotations are simply compounded to produce the final operation that orients the body in space, and any individual rotation can be undone by applying the inverse operation.

You are close and just need to do some more reading, but here is how to do what you want using 3x3 rotation matrices, where the inverse of a rotation operator is its transpose.

1. current orientation: b = R0*w (w is a vector in the world frame, b is a vector in the body frame).
2. save R0, defines current orientation
3. new orientation: b' = R'*w
4. R' is just R1*R0 (compounded operation), and you want R1, the operator that takes b to b'
5. R1 = R'*R0(T) (left matrix multiplication with the inverse of R0, its transpose).
6. If you like, decompose R1 to the relative orientation angles that relate b and b'

A similar approach works with quaternions.