В случае откалиброванных данных с акселерометра и магнитометра:
float heading;
float P,R;
R = atan2f(aсс.y(), aсс.z()); //Roll
float V = sqrt(aсс.y()*aсс.y()+aсс.z()*aсс.z());
P = atan2f(aсс.x(), V); //Pitch
Matrix_3x3_t RM;
RM = CalcRMatrix(R, P, 0);
QVector3D mag_rotated = Rotate3Axis(mag_vector, RM);
heading = atan2(mag_rotated.y(), mag_rotated.x());
//---------------------------------------------------------
Matrix_3x3_t CalcRMatrix(float alpha, float beta, float gamma)
{
Matrix_3x3_t RotMatrix;
float sina = sin(alpha);
float cosa = cos(alpha);
float sinb = sin(beta);
float cosb = cos(beta);
float sing = sin(gamma);
float cosg = cos(gamma);
// x y
// c r
RotMatrix.cell[0][0] = cosb*cosg;
RotMatrix.cell[0][1] = sina*sinb*cosg+cosa*sing;
RotMatrix.cell[0][2] = -cosa*sinb*cosg+sina*sing;
RotMatrix.cell[1][0] = -cosb*sing;
RotMatrix.cell[1][1] = -sina*sinb*sing+cosa*cosg;
RotMatrix.cell[1][2] = cosa*sinb*sing+sina*cosg;
RotMatrix.cell[2][0] = sinb;
RotMatrix.cell[2][1] = -sina*cosb;
RotMatrix.cell[2][2] = cosa*cosb;
return RotMatrix;
}
QVector3D Rotate3Axis(QVector3D point, Matrix_3x3_t RotMatrix)
{
QVector3D v;
float x,y,z;
x = point.x();
y = point.y();
z = point.z();
float x1,y1,z1;
x1 = x*RotMatrix.cell[0][0] + y*RotMatrix.cell[1][0] + z*RotMatrix.cell[2][0];
y1 = x*RotMatrix.cell[0][1] + y*RotMatrix.cell[1][1] + z*RotMatrix.cell[2][1];
z1 = x*RotMatrix.cell[0][2] + y*RotMatrix.cell[1][2] + z*RotMatrix.cell[2][2];
v.setX(x1);
v.setY(y1);
v.setZ(z1);
return v;
}
Выдрано из тестов, но думаю идея понятна
