Hi,
I use 9250 with DMP enabled to get LP_QUAT_6.
This function give me YAW PITCH ROLL , i pass the quaternion that i got from the dmp.
public static double[] getYawPitchRoll(this double[] q)
{
double gx, gy, gz;
var ypr = new double[3];
gx = 2d * (q[1] * q[3] – q[0] * q[2]);
gy = 2d * (q[0] * q[1] + q[2] * q[3]);
gz = q[0] * q[0] – q[1] * q[1] – q[2] * q[2] + q[3] * q[3];
ypr[0] = Math.Atan2(2d * q[1] * q[2] – 2d * q[0] * q[3], 2d * q[0] * q[0] + 2d * q[1] * q[1] – 1d);
ypr[1] = Math.Atan(gx / Math.Sqrt(gy * gy + gz * gz));
ypr[2] = Math.Atan(gy / Math.Sqrt(gx * gx + gz * gz));
return ypr;
}
Turning on ROLL,PITCH 90 ° gives me a delta about 90 ° , but turning on YAW 90 ° gives me a delta about 45 °
If i use with the DMP’s quaternion and magnetometer i got a right angle result but very noisy.
static double calcMagHeading(this double[] q, double bx, double by, double bz)
{
double Head_X, Head_Y;
double cos_roll, sin_roll, cos_pitch, sin_pitch;
double gx, gy, gz; // estimated gravity direction
var ypr = new double[3];
double q0 = q[0];
double q1 = q[1];
double q2 = q[2];
double q3 = q[3];
gx = 2d * (q1 * q3 – q0 * q2);
gy = 2d * (q0 * q1 + q2 * q3);
gz = q0 * q0 – q1 * q1 – q2 * q2 + q3 * q3;
//ypr[0] = Math.Atan2(2 * q1 * q2 – 2 * q0 * q3, 2 * q0 * q0 + 2 * q1 * q1 – 1);
ypr[1] = Math.Atan(gx / Math.Sqrt(gy * gy + gz * gz));
ypr[2] = Math.Atan(gy / Math.Sqrt(gx * gx + gz * gz));
cos_roll = Math.Cos(-ypr[2]);
sin_roll = Math.Sin(-ypr[2]);
cos_pitch = Math.Cos(ypr[1]);
sin_pitch = Math.Sin(ypr[1]);
// Tilt compensated Magnetic field X component:
Head_X = bx * cos_pitch + by * sin_roll * sin_pitch + bz * cos_roll * sin_pitch;
// Tilt compensated Magnetic field Y component:
Head_Y = by * cos_roll – bz * sin_roll;
// Magnetic Heading
return (Math.Atan2(-Head_Y, Head_X) ) ;
}
What’s wrong ?
Thank you