How do I convert quaternion values to specific angles

时间:2019-01-15 18:28:19

标签: angle quaternions

I'm getting quaternion values, and I want to convert these values to 2 angles.
One of the angles is between the "ground plane" (please correct me if this is an incorrect name for it) and the fictional vector between the origin and the quaternion point ( the complementary angle of gamma ).
The second angle is the rotational angle, this is the rotation that the vector has made relatively to a plane perpendicular to the vector ( R in the picture ).

I looked lot up on the internet, but everything I try has a range of -90° to 90°, while I want it to go from -180° to 180°.

This is the last version of code I ended up with:

float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan(gx / sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan(gy/gz); // about X axis

Serial.print("  yaw ");
Serial.print(yaw * 180/M_PI,0);
Serial.print("  pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print("  sideways ");
// Don't mind this section as I'm applying a mathematical function I created
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);

Picture

1 个答案:

答案 0 :(得分:0)

  

您应该始终使用atan2(y,x)而不是atan(y / x)。这是一个常见的错误。 –索莫斯

当我也问他的时候,他是用数学表格写的,那是我愚蠢的错误-_-

我的新版本是:

float gx = 2 * (x*z - w*y);
float gy = 2 * (w*x + y*z);
float gz = w*w - x*x - y*y + z*z;
float yaw = atan2(2*x*y - 2*w*z, 2*w*w + 2*x*x - 1); // about Z axis
float pitch = atan2(gx, sqrt(gy*gy + gz*gz)); // about Y axis
float roll = atan2(gy, gz); // about X axis

/*Serial.print("  yaw ");
Serial.print(yaw * 180/M_PI,0);*/
Serial.print("  pitch ");
Serial.print(pitch * 180/M_PI,2);
Serial.print("  sideways ");
// Please don't pay  attention to the extra function I made for the project but it doesn't have to do with the problem
if(pitch > 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,((pitch * 180/M_PI)-51.57)))), 2);
else if(pitch == 0) Serial.println(roll * 180/M_PI, 2);
else if(pitch < 0) Serial.println((roll * 180/M_PI) * (1/(1+pow(1.293,(((pitch) * (-180)/M_PI)-51.57)))), 2);