Calculating Yaw Pitch and Roll from Quaternions in MPU6050 without Gimback lock but giving problems
This code did work at least for what we needed in our humanoid robot, thanks for comments to everyone specially to AJN.
mpu.dmpGetQuaternion(&amp;q, fifoBuffer);
float heading;
float attitude;
float bank;
double sqw = q.w * q.w;
double sqx = q.x * q.x;
double sqy = q.y * q.y;
double sqz = q.z * q.z;    
//Heading is like yaw, works ok
//attitude should be pitch but is working as roll?? only works correctly in a 180~ degree range
//Bank should be roll but is working as pitch??, works ok 
heading = atan2(2.0 * (q.w*q.z + q.x*q.y),1.0 - 2.0 * (sqy + sqz));
attitude = asin(2 * (q.w * q.y - q.z * q.x));
bank = atan2(2.0 * q.x * q.w -2 * q.y * q.z,1.0 - 2.0 * sqx - 2 * sqz);
Serial.print(&quot;conversion\t&quot;);    
Serial.print(heading * 180 / M_PI);
Serial.print(&quot;\t&quot;);
Serial.print(attitude * 180 / M_PI);
Serial.print(&quot;\t&quot;);
Serial.println(bank * 180 / M_PI);