roll and pitch angles ranges

[hassan ali]

Hi everybody... I've been working on a project recently and i need to measure the roll and pitch angles of a moving platform, but the problem is that the value of these angles starts from 0 then reaches 90, then starts to decrease again to reach 0 (positive or negative 90 degrees of course) ... what i need to do is to have this value in the range of zero to 180 (positive or negative) not from zero to 90 as it is now ... do anyone have any idea??

[tsimon]

Hi,

 

I am seeing that you can do a full rotation around the z axis using the dmu and get accurate angles. But if you try to read the angle from the x or y axis it only goes to 90 degrees or gives garbabge. It seems that the dmu is making some assumption about the mounting position of the unit and the expected motion. My problem is that I want to read full rotattion from the x axis. I amssuming the gryos are reading correctly and this issue is in the dmu code that generates the quaternion.  I am using Euler angles and have also tried YPR. They both behave the same.

[Jeff Rowberg]

I amssuming the gryos are reading correctly and this issue is in the dmu code that generates the quaternion.  I am using Euler angles and have also tried YPR. They both behave the same.

 

The main problem is (fortunately) not inside the DMP binary block, since its only job is to generate quaternions, which behave perfectly. The problem is in the math in the two conversion functions in MPU6050_6Axis_MotionApps20.h starting on line 639:

 

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h#L639

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;
}
uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
    // 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));
    return 0;
}

I got those calculations from somewhere else, but quaternions have always been a bit of a struggle for me and the conversion to Euler or YPR angles is definitely more of a magic black box to me than anything else. If someone more familiar with the required math can improve these, or find other sources which do the conversion properly, this is where the fix would need to go.

[cartola]

Hi Jeff,

 

First: Congratulations and thank you for the amazing work!

 

I would like to know if there is any news about this topic. 

 

Moreover, if I can add a related question to this topic, the range of Pitch and Roll seems to vary from 0 to ~82. When pitch or roll reach more or less 82, it start to decrease. It never reaches ~89. Is this expected? Actually, it is not a big problem to me, because I might map 0->82 to 0->90, but I am curious to know if I am doing/interpreting something wrong. I am using the example code.

 

Again, thank you!!!

 

PS: English is not my maternal language...

 

Eduardo

[hatt0002]

Hi,

Are there any news about this topic? I have the same problem and I need the pitch and roll to have a range 0 to 180 and 0 to -180. Did someone managed to do the conversion? 

Thanks, Hatt

[cartola]

Hi,

Are there any news about this topic? I have the same problem and I need the pitch and roll to have a range 0 to 180 and 0 to -180. Did someone managed to do the conversion? 

Thanks, Hatt

 

Nothing here... Please, let me know if you have any news.

 

Best,

Eduardo

[hatt0002]

Ok I have managed to solve the pitch and roll range issue. The pitch ranges from 1 to 90 and from 90 to zero. Also from 0 to -90 and from -90 to 0. I have noted that the transition at (89) to (90) to (89) and (-89) to (-90) to (-89) takes place when the gravity component in the z direction (gravity[2]) changes from positive to negative. At this instant, I am negating the value of the pitch/roll and subtracting a PI.  In this way, I am getting a 0-180 range for pitch and roll. Furthermore, If you want a 0-360 range, just subtract a 2PI when the current pitch/roll is greater than 0. this will give a range from 0 to -360. Take the absolute value. Here is my code.

//calculating Pitch1
            if(gravity[2]>0) //range from 0 to 90 and from 0 to -90
            {
              ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
            }
            else if(gravity[2]<0) //to get range from 90 to 180 and ftom -90 to -180
            {
              ypr[1] =  - (PI)-atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
            } 
            
              if(ypr[1] > 0) //to get range from 0 to -360
              {
                ypr[1] = -2*PI + ypr[1];
              }

              ypr[1] = abs(ypr[1]);
            
            //calculating Roll1
            if(gravity[2]>0) 
            {
              ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
            }
            else if(gravity[2]<0)
            {
              ypr[2] = -PI - atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2]));
            }
              if(ypr[2] > 0) 
              {
                ypr[2] = -2*PI + ypr[2];
              }
              
              ypr[2] = abs(ypr[2]);

Now my problem is the yaw is affected by the roll and pitch. I mean that when I change the roll without changing yaw, yaw still changes. For example on a 45 degree roll, the yaw changes by 10 degrees. Is this normal? Maybe Yaw is dependent on gravity component which I have to remove? If this is normal, is there a way to make yaw, pitch and roll independent on each other?
Can someone please help since I need this issue to be solved as soon as possible.

Thanks, Hatt

[cartola]

Hi Hatt0002,

 

Nice work! Definitely I'll take a look on it, but I can't answer your question about the yaw.

[Fasteddie]

Hello Hatt

I think this is what I have been looking for.

Can you tell me where this code should be pasted

Thanks

Ed

[beng27]

HI, I've been working on this and I offer a better coding of the  conversion function in MPU6050_6Axis_MotionApps20.h starting on line 645. 

 

This gives the correct readings of +- 180 degrees in all 3 axes in the stock demo code

 

This was figured out using info from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm

and crucially the discussion at http://stackoverflow.com/questions/3755059/3d-accelerometer-calculate-the-orientation

 

Please note: 1) Normally I develop Filemaker Databases and I  really don't understand quaternions either!  2) I've only been using the Ardiono for a week or so and never seen C++ before, it may be that my code is not efficient. However I think the logic is sound.

 

Ben

 

_______

int8_t    sign;

uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
    // p & r need to check for >90 rotation and correct for gravity polarity reversal
    if (gravity -> z > 0) {sign = 1;} else {sign = -1;}
    // 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)
    // adding tiny fraction of x2 or y2  avoids gimbal lock at cost of slight accuracy
    data[1] = atan2(gravity -> x , sign * sqrt(0.001*gravity -> y*gravity -> y + gravity -> z*gravity -> z));
    // roll: (tilt left/right, about X axis)   
    data[2] = atan2(gravity -> y , sign * sqrt(0.001*gravity -> x*gravity -> x + gravity -> z*gravity -> z));
  return 0;
}