orientation.c

Go to the documentation of this file.

/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */
 
// This file contains functions designed to operate on, or compute, orientations.
// These may be in rotation matrix form, quaternion form, or Euler angles.
// It also includes functions designed to operate with specify reference frames
// (Android, Windows 8, NED).
//
 
#include <math.h>
 
#include "sensor_fusion.h"
#include "orientation.h"
#include "matrix.h"
#include "approximations.h"
 
// compile time constants that are private to this file
#define SMALLQ0 1E-4F       // limit of quaternion scalar component requiring special algorithm
#define CORRUPTQUAT 0.001F  // threshold for deciding rotation quaternion is corrupt
#define SMALLMODULUS 0.01F  // limit where rounding errors may appear
 
#if F_USING_ACCEL  // Need tilt conversion routines
// Aerospace NED accelerometer 3DOF tilt function computing rotation matrix fR
#if (THISCOORDSYSTEM == NED) || (THISCOORDSYSTEM == ANDROID)
void f3DOFTiltNED(float fR[][3], float fGc[])
{
    // the NED self-consistency twist occurs at 90 deg pitch
 
    // local variables
    int16_t i;              // counter
    float fmodGxyz;         // modulus of the x, y, z accelerometer readings
    float fmodGyz;          // modulus of the y, z accelerometer readings
    float frecipmodGxyz;    // reciprocal of modulus
    float ftmp;             // scratch variable
 
    // compute the accelerometer squared magnitudes
    fmodGyz = fGc[CHY] * fGc[CHY] + fGc[CHZ] * fGc[CHZ];
    fmodGxyz = fmodGyz + fGc[CHX] * fGc[CHX];
 
    // check for freefall special case where no solution is possible
    if (fmodGxyz == 0.0F)
    {
        f3x3matrixAeqI(fR);
        return;
    }
 
    // check for vertical up or down gimbal lock case
    if (fmodGyz == 0.0F)
    {
        f3x3matrixAeqScalar(fR, 0.0F);
        fR[CHY][CHY] = 1.0F;
        if (fGc[CHX] >= 0.0F)
        {
            fR[CHX][CHZ] = 1.0F;
            fR[CHZ][CHX] = -1.0F;
        }
        else
        {
            fR[CHX][CHZ] = -1.0F;
            fR[CHZ][CHX] = 1.0F;
        }
        return;
    }
 
    // compute moduli for the general case
    fmodGyz = sqrtf(fmodGyz);
    fmodGxyz = sqrtf(fmodGxyz);
    frecipmodGxyz = 1.0F / fmodGxyz;
    ftmp = fmodGxyz / fmodGyz;
 
    // normalize the accelerometer reading into the z column
    for (i = CHX; i <= CHZ; i++)
    {
        fR[i][CHZ] = fGc[i] * frecipmodGxyz;
    }
 
    // construct x column of orientation matrix
    fR[CHX][CHX] = fmodGyz * frecipmodGxyz;
    fR[CHY][CHX] = -fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
    fR[CHZ][CHX] = -fR[CHX][CHZ] * fR[CHZ][CHZ] * ftmp;
 
    // construct y column of orientation matrix
    fR[CHX][CHY] = 0.0F;
    fR[CHY][CHY] = fR[CHZ][CHZ] * ftmp;
    fR[CHZ][CHY] = -fR[CHY][CHZ] * ftmp;
 
    return;
}
#endif // #if THISCOORDSYSTEM == NED
 
// Android accelerometer 3DOF tilt function computing rotation matrix fR
#if THISCOORDSYSTEM == ANDROID
void f3DOFTiltAndroid(float fR[][3], float fGc[])
{
    // the Android tilt matrix is mathematically identical to the NED tilt matrix
    // the Android self-consistency twist occurs at 90 deg roll
    f3DOFTiltNED(fR, fGc);
    return;
}
#endif // #if THISCOORDSYSTEM == ANDROID
 
// Windows 8 accelerometer 3DOF tilt function computing rotation matrix fR
#if (THISCOORDSYSTEM == WIN8) || (THISCOORDSYSTEM == ANDROID)
void f3DOFTiltWin8(float fR[][3], float fGc[])
{
    // the Win8 self-consistency twist occurs at 90 deg roll
 
    // local variables
    float fmodGxyz;         // modulus of the x, y, z accelerometer readings
    float fmodGxz;          // modulus of the x, z accelerometer readings
    float frecipmodGxyz;    // reciprocal of modulus
    float ftmp;             // scratch variable
    int8 i;                 // counter
 
    // compute the accelerometer squared magnitudes
    fmodGxz = fGc[CHX] * fGc[CHX] + fGc[CHZ] * fGc[CHZ];
    fmodGxyz = fmodGxz + fGc[CHY] * fGc[CHY];
 
    // check for freefall special case where no solution is possible
    if (fmodGxyz == 0.0F)
    {
        f3x3matrixAeqI(fR);
        return;
    }
 
    // check for vertical up or down gimbal lock case
    if (fmodGxz == 0.0F)
    {
        f3x3matrixAeqScalar(fR, 0.0F);
        fR[CHX][CHX] = 1.0F;
        if (fGc[CHY] >= 0.0F)
        {
            fR[CHY][CHZ] = -1.0F;
            fR[CHZ][CHY] = 1.0F;
        }
        else
        {
            fR[CHY][CHZ] = 1.0F;
            fR[CHZ][CHY] = -1.0F;
        }
        return;
    }
 
    // compute moduli for the general case
    fmodGxz = sqrtf(fmodGxz);
    fmodGxyz = sqrtf(fmodGxyz);
    frecipmodGxyz = 1.0F / fmodGxyz;
    ftmp = fmodGxyz / fmodGxz;
    if (fGc[CHZ] < 0.0F)
    {
        ftmp = -ftmp;
    }
 
    // normalize the negated accelerometer reading into the z column
    for (i = CHX; i <= CHZ; i++)
    {
        fR[i][CHZ] = -fGc[i] * frecipmodGxyz;
    }
 
    // construct x column of orientation matrix
    fR[CHX][CHX] = -fR[CHZ][CHZ] * ftmp;
    fR[CHY][CHX] = 0.0F;
    fR[CHZ][CHX] = fR[CHX][CHZ] * ftmp;;
 
    // // construct y column of orientation matrix
    fR[CHX][CHY] = fR[CHX][CHZ] * fR[CHY][CHZ] * ftmp;
    fR[CHY][CHY] = -fmodGxz * frecipmodGxyz;
    if (fGc[CHZ] < 0.0F)
    {
        fR[CHY][CHY] = -fR[CHY][CHY];
    }
    fR[CHZ][CHY] = fR[CHY][CHZ] * fR[CHZ][CHZ] * ftmp;
 
    return;
}
#endif // #if (THISCOORDSYSTEM == WIN8)
#endif // #if F_USING_ACCEL  // Need tilt conversion routines
 
#if F_USING_MAG // Need eCompass conversion routines
// Aerospace NED magnetometer 3DOF flat eCompass function computing rotation matrix fR
#if THISCOORDSYSTEM == NED
void f3DOFMagnetometerMatrixNED(float fR[][3], float fBc[])
{
    // local variables
    float fmodBxy;          // modulus of the x, y magnetometer readings
 
    // compute the magnitude of the horizontal (x and y) magnetometer reading
    fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
 
    // check for zero field special case where no solution is possible
    if (fmodBxy == 0.0F)
    {
        f3x3matrixAeqI(fR);
        return;
    }
 
    // define the fixed entries in the z row and column
    fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
    fR[CHZ][CHZ] = 1.0F;
 
    // define the remaining entries
    fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHX] / fmodBxy;
    fR[CHY][CHX] = fBc[CHY] / fmodBxy;
    fR[CHX][CHY] = -fR[CHY][CHX];
 
    return;
}
#endif // #if THISCOORDSYSTEM == NED
 
// Android magnetometer 3DOF flat eCompass function computing rotation matrix fR
#if (THISCOORDSYSTEM == ANDROID) || (THISCOORDSYSTEM == WIN8)
void f3DOFMagnetometerMatrixAndroid(float fR[][3], float fBc[])
{
    // local variables
    float fmodBxy;          // modulus of the x, y magnetometer readings
 
    // compute the magnitude of the horizontal (x and y) magnetometer reading
    fmodBxy = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY]);
 
    // check for zero field special case where no solution is possible
    if (fmodBxy == 0.0F)
    {
        f3x3matrixAeqI(fR);
        return;
    }
 
    // define the fixed entries in the z row and column
    fR[CHZ][CHX] = fR[CHZ][CHY] = fR[CHX][CHZ] = fR[CHY][CHZ] = 0.0F;
    fR[CHZ][CHZ] = 1.0F;
 
    // define the remaining entries
    fR[CHX][CHX] = fR[CHY][CHY] = fBc[CHY] / fmodBxy;
    fR[CHX][CHY] = fBc[CHX] / fmodBxy;
    fR[CHY][CHX] = -fR[CHX][CHY];
 
    return;
}
#endif // #if THISCOORDSYSTEM == ANDROID
 
// Windows 8 magnetometer 3DOF flat eCompass function computing rotation matrix fR
#if (THISCOORDSYSTEM == WIN8)
void f3DOFMagnetometerMatrixWin8(float fR[][3], float fBc[])
{
    // call the Android function since it is identical to the Windows 8 matrix
    f3DOFMagnetometerMatrixAndroid(fR, fBc);
 
    return;
}
#endif // #if (THISCOORDSYSTEM == WIN8)
 
// NED: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
#if THISCOORDSYSTEM == NED
void feCompassNED(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[], float *pfmodBc, float *pfmodGc)
{
    // local variables
    float fmod[3];                  // column moduli
    float fGcdotBc;                 // dot product of vectors G.Bc
    float ftmp;                     // scratch variable
    int8_t i, j;                        // loop counters
 
    // set the inclination angle to zero in case it is not computed later
    *pfDelta = *pfsinDelta = 0.0F;
    *pfcosDelta = 1.0F;
 
    // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and x axes
    for (i = CHX; i <= CHZ; i++)
    {
        fR[i][CHZ] = fGc[i];
        fR[i][CHX] = fBc[i];
    }
 
    // set y vector to vector product of z and x vectors
    fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
    fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
    fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
 
    // set x vector to vector product of y and z vectors
    fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
    fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
    fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
 
    // calculate the rotation matrix column moduli
    fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
    fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
    fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
 
    // normalize the rotation matrix columns
    if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
    {
        // loop over columns j
        for (j = CHX; j <= CHZ; j++)
        {
            ftmp = 1.0F / fmod[j];
            // loop over rows i
            for (i = CHX; i <= CHZ; i++)
            {
                // normalize by the column modulus
                fR[i][j] *= ftmp;
            }
        }
    }
    else
    {
        // no solution is possible so set rotation to identity matrix
        f3x3matrixAeqI(fR);
        return;
    }
 
    // compute the geomagnetic inclination angle (deg)
    *pfmodGc = fmod[CHZ];
    *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
    fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
    if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
    {
        *pfsinDelta = fGcdotBc / (*pfmodGc * *pfmodBc);
        *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
        *pfDelta = fasin_deg(*pfsinDelta);
    }
 
    return;
}
#endif // #if THISCOORDSYSTEM == NED
 
// Android: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
#if THISCOORDSYSTEM == ANDROID
void feCompassAndroid(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[],
        float *pfmodBc, float *pfmodGc)
{
    // local variables
    float fmod[3];                  // column moduli
    float fGcdotBc;                 // dot product of vectors G.Bc
    float ftmp;                     // scratch variable
    int8 i, j;                      // loop counters
 
    // set the inclination angle to zero in case it is not computed later
    *pfDelta = *pfsinDelta = 0.0F;
    *pfcosDelta = 1.0F;
 
    // place the un-normalized gravity and geomagnetic vectors into the rotation matrix z and y axes
    for (i = CHX; i <= CHZ; i++)
    {
        fR[i][CHZ] = fGc[i];
        fR[i][CHY] = fBc[i];
    }
 
    // set x vector to vector product of y and z vectors
    fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
    fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
    fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
 
    // set y vector to vector product of z and x vectors
    fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
    fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
    fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
 
    // calculate the rotation matrix column moduli
    fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
    fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
    fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
 
    // normalize the rotation matrix columns
    if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
    {
        // loop over columns j
        for (j = CHX; j <= CHZ; j++)
        {
            ftmp = 1.0F / fmod[j];
            // loop over rows i
            for (i = CHX; i <= CHZ; i++)
            {
                // normalize by the column modulus
                fR[i][j] *= ftmp;
            }
        }
    }
    else
    {
        // no solution is possible so set rotation to identity matrix
        f3x3matrixAeqI(fR);
        return;
    }
 
    // compute the geomagnetic inclination angle (deg)
    *pfmodGc = fmod[CHZ];
    *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
    fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
    if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
    {
        *pfsinDelta = -fGcdotBc / (*pfmodGc * *pfmodBc);
        *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
        *pfDelta = fasin_deg(*pfsinDelta);
    }
 
    return;
}
#endif // #if THISCOORDSYSTEM == ANDROID
 
// Win8: basic 6DOF e-Compass function computing rotation matrix fR and magnetic inclination angle fDelta
#if (THISCOORDSYSTEM == WIN8)
void feCompassWin8(float fR[][3], float *pfDelta, float *pfsinDelta, float *pfcosDelta, float fBc[], float fGc[],
        float *pfmodBc, float *pfmodGc)
{
    // local variables
    float fmod[3];                  // column moduli
    float fGcdotBc;                 // dot product of vectors G.Bc
    float ftmp;                     // scratch variable
    int8 i, j;                      // loop counters
 
    // set the inclination angle to zero in case it is not computed later
    *pfDelta = *pfsinDelta = 0.0F;
    *pfcosDelta = 1.0F;
 
    // place the negated un-normalized gravity and un-normalized geomagnetic vectors into the rotation matrix z and y axes
    for (i = CHX; i <= CHZ; i++)
    {
        fR[i][CHZ] = -fGc[i];
        fR[i][CHY] = fBc[i];
    }
 
    // set x vector to vector product of y and z vectors
    fR[CHX][CHX] = fR[CHY][CHY] * fR[CHZ][CHZ] - fR[CHZ][CHY] * fR[CHY][CHZ];
    fR[CHY][CHX] = fR[CHZ][CHY] * fR[CHX][CHZ] - fR[CHX][CHY] * fR[CHZ][CHZ];
    fR[CHZ][CHX] = fR[CHX][CHY] * fR[CHY][CHZ] - fR[CHY][CHY] * fR[CHX][CHZ];
 
    // set y vector to vector product of z and x vectors
    fR[CHX][CHY] = fR[CHY][CHZ] * fR[CHZ][CHX] - fR[CHZ][CHZ] * fR[CHY][CHX];
    fR[CHY][CHY] = fR[CHZ][CHZ] * fR[CHX][CHX] - fR[CHX][CHZ] * fR[CHZ][CHX];
    fR[CHZ][CHY] = fR[CHX][CHZ] * fR[CHY][CHX] - fR[CHY][CHZ] * fR[CHX][CHX];
 
    // calculate the rotation matrix column moduli
    fmod[CHX] = sqrtf(fR[CHX][CHX] * fR[CHX][CHX] + fR[CHY][CHX] * fR[CHY][CHX] + fR[CHZ][CHX] * fR[CHZ][CHX]);
    fmod[CHY] = sqrtf(fR[CHX][CHY] * fR[CHX][CHY] + fR[CHY][CHY] * fR[CHY][CHY] + fR[CHZ][CHY] * fR[CHZ][CHY]);
    fmod[CHZ] = sqrtf(fR[CHX][CHZ] * fR[CHX][CHZ] + fR[CHY][CHZ] * fR[CHY][CHZ] + fR[CHZ][CHZ] * fR[CHZ][CHZ]);
 
    // normalize the rotation matrix columns
    if (!((fmod[CHX] == 0.0F) || (fmod[CHY] == 0.0F) || (fmod[CHZ] == 0.0F)))
    {
        // loop over columns j
        for (j = CHX; j <= CHZ; j++)
        {
            ftmp = 1.0F / fmod[j];
            // loop over rows i
            for (i = CHX; i <= CHZ; i++)
            {
                // normalize by the column modulus
                fR[i][j] *= ftmp;
            }
        }
    }
    else
    {
        // no solution is possible so set rotation to identity matrix
        f3x3matrixAeqI(fR);
        return;
    }
 
    // compute the geomagnetic inclination angle (deg)
    *pfmodGc = fmod[CHZ];
    *pfmodBc = sqrtf(fBc[CHX] * fBc[CHX] + fBc[CHY] * fBc[CHY] + fBc[CHZ] * fBc[CHZ]);
    fGcdotBc = fGc[CHX] * fBc[CHX] + fGc[CHY] * fBc[CHY] + fGc[CHZ] * fBc[CHZ];
    if (!((*pfmodGc == 0.0F) || (*pfmodBc == 0.0F)))
    {
        *pfsinDelta = fGcdotBc / (*pfmodGc * *pfmodBc);
        *pfcosDelta = sqrtf(1.0F - *pfsinDelta * *pfsinDelta);
        *pfDelta = fasin_deg(*pfsinDelta);
    }
 
    return;
}
#endif // #if (THISCOORDSYSTEM == WIN8)
#endif // #if F_USING_MAG // Need eCompass conversion routines
 
// extract the NED angles in degrees from the NED rotation matrix
#if THISCOORDSYSTEM == NED
void fNEDAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
        float *pfRhoDeg, float *pfChiDeg)
{
    // calculate the pitch angle -90.0 <= Theta <= 90.0 deg
    *pfTheDeg = fasin_deg(-R[CHX][CHZ]);
 
    // calculate the roll angle range -180.0 <= Phi < 180.0 deg
    *pfPhiDeg = fatan2_deg(R[CHY][CHZ], R[CHZ][CHZ]);
 
    // map +180 roll onto the functionally equivalent -180 deg roll
    if (*pfPhiDeg == 180.0F)
    {
        *pfPhiDeg = -180.0F;
    }
 
    // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
    if (*pfTheDeg == 90.0F)
    {
        // vertical upwards gimbal lock case
        *pfPsiDeg = fatan2_deg(R[CHZ][CHY], R[CHY][CHY]) + *pfPhiDeg;
    }
    else if (*pfTheDeg == -90.0F)
    {
        // vertical downwards gimbal lock case
        *pfPsiDeg = fatan2_deg(-R[CHZ][CHY], R[CHY][CHY]) - *pfPhiDeg;
    }
    else
    {
        // general case
        *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]);
    }
 
    // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
    if (*pfPsiDeg < 0.0F)
    {
        *pfPsiDeg += 360.0F;
    }
 
    // check for rounding errors mapping small negative angle to 360 deg
    if (*pfPsiDeg >= 360.0F)
    {
        *pfPsiDeg = 0.0F;
    }
 
    // for NED, the compass heading Rho equals the yaw angle Psi
    *pfRhoDeg = *pfPsiDeg;
 
    // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
    *pfChiDeg = facos_deg(R[CHZ][CHZ]);
 
    return;
}
#endif // #if THISCOORDSYSTEM == NED
 
// extract the Android angles in degrees from the Android rotation matrix
#if THISCOORDSYSTEM == ANDROID
void fAndroidAnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
        float *pfRhoDeg, float *pfChiDeg)
{
    // calculate the roll angle -90.0 <= Phi <= 90.0 deg
    *pfPhiDeg = fasin_deg(R[CHX][CHZ]);
 
    // calculate the pitch angle -180.0 <= The < 180.0 deg
    *pfTheDeg = fatan2_deg(-R[CHY][CHZ], R[CHZ][CHZ]);
 
    // map +180 pitch onto the functionally equivalent -180 deg pitch
    if (*pfTheDeg == 180.0F)
    {
        *pfTheDeg = -180.0F;
    }
 
    // calculate the yaw (compass) angle 0.0 <= Psi < 360.0 deg
    if (*pfPhiDeg == 90.0F)
    {
        // vertical downwards gimbal lock case
        *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) - *pfTheDeg;
    }
    else if (*pfPhiDeg == -90.0F)
    {
        // vertical upwards gimbal lock case
        *pfPsiDeg = fatan2_deg(R[CHY][CHX], R[CHY][CHY]) + *pfTheDeg;
    }
    else
    {
        // general case
        *pfPsiDeg = fatan2_deg(-R[CHX][CHY], R[CHX][CHX]);
    }
 
    // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
    if (*pfPsiDeg < 0.0F)
    {
        *pfPsiDeg += 360.0F;
    }
 
    // check for rounding errors mapping small negative angle to 360 deg
    if (*pfPsiDeg >= 360.0F)
    {
        *pfPsiDeg = 0.0F;
    }
 
    // the compass heading angle Rho equals the yaw angle Psi
    // this definition is compliant with Motorola Xoom tablet behavior
    *pfRhoDeg = *pfPsiDeg;
 
    // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
    *pfChiDeg = facos_deg(R[CHZ][CHZ]);
 
    return;
}
#endif // #if THISCOORDSYSTEM == ANDROID
 
// extract the Windows 8 angles in degrees from the Windows 8 rotation matrix
#if (THISCOORDSYSTEM == WIN8)
void fWin8AnglesDegFromRotationMatrix(float R[][3], float *pfPhiDeg, float *pfTheDeg, float *pfPsiDeg,
        float *pfRhoDeg, float *pfChiDeg)
{
    // calculate the roll angle -90.0 <= Phi <= 90.0 deg
    if (R[CHZ][CHZ] == 0.0F)
    {
        if (R[CHX][CHZ] >= 0.0F)
        {
            // tan(phi) is -infinity
            *pfPhiDeg = -90.0F;
        }
        else
        {
            // tan(phi) is +infinity
            *pfPhiDeg = 90.0F;
        }
    }
    else
    {
        // general case
        *pfPhiDeg = fatan_deg(-R[CHX][CHZ] / R[CHZ][CHZ]);
    }
 
    // first calculate the pitch angle The in the range -90.0 <= The <= 90.0 deg
    *pfTheDeg = fasin_deg(R[CHY][CHZ]);
 
    // use R[CHZ][CHZ]=cos(Phi)*cos(The) to correct the quadrant of The remembering
    // cos(Phi) is non-negative so that cos(The) has the same sign as R[CHZ][CHZ].
    if (R[CHZ][CHZ] < 0.0F)
    {
        // wrap The around +90 deg and -90 deg giving result 90 to 270 deg
        *pfTheDeg = 180.0F - *pfTheDeg;
    }
 
    // map the pitch angle The to the range -180.0 <= The < 180.0 deg
    if (*pfTheDeg >= 180.0F)
    {
        *pfTheDeg -= 360.0F;
    }
 
    // calculate the yaw angle Psi
    if (*pfTheDeg == 90.0F)
    {
        // vertical upwards gimbal lock case: -270 <= Psi < 90 deg
        *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) - *pfPhiDeg;
    }
    else if (*pfTheDeg == -90.0F)
    {
        // vertical downwards gimbal lock case: -270 <= Psi < 90 deg
        *pfPsiDeg = fatan2_deg(R[CHX][CHY], R[CHX][CHX]) + *pfPhiDeg;
    }
    else
    {
        // general case: -180 <= Psi < 180 deg
        *pfPsiDeg = fatan2_deg(-R[CHY][CHX], R[CHY][CHY]);
 
        // correct the quadrant for Psi using the value of The (deg) to give -180 <= Psi < 380 deg
        if (fabsf(*pfTheDeg) >= 90.0F)
        {
            *pfPsiDeg += 180.0F;
        }
    }
 
    // map yaw angle Psi onto range 0.0 <= Psi < 360.0 deg
    if (*pfPsiDeg < 0.0F)
    {
        *pfPsiDeg += 360.0F;
    }
 
    // check for any rounding error mapping small negative angle to 360 deg
    if (*pfPsiDeg >= 360.0F)
    {
        *pfPsiDeg = 0.0F;
    }
 
    // compute the compass angle Rho = 360 - Psi
    *pfRhoDeg = 360.0F - *pfPsiDeg;
 
    // check for rounding errors mapping small negative angle to 360 deg and zero degree case
    if (*pfRhoDeg >= 360.0F)
    {
        *pfRhoDeg = 0.0F;
    }
 
    // calculate the tilt angle from vertical Chi (0 <= Chi <= 180 deg)
    *pfChiDeg = facos_deg(R[CHZ][CHZ]);
 
    return;
}
#endif // #if (THISCOORDSYSTEM == WIN8)
 
// computes normalized rotation quaternion from a rotation vector (deg)
void fQuaternionFromRotationVectorDeg(Quaternion *pq, const float rvecdeg[], float fscaling)
{
    float fetadeg;          // rotation angle (deg)
    float fetarad;          // rotation angle (rad)
    float fetarad2;         // eta (rad)^2
    float fetarad4;         // eta (rad)^4
    float sinhalfeta;       // sin(eta/2)
    float fvecsq;           // q1^2+q2^2+q3^2
    float ftmp;             // scratch variable
 
    // compute the scaled rotation angle eta (deg) which can be both positve or negative
    fetadeg = fscaling * sqrtf(rvecdeg[CHX] * rvecdeg[CHX] + rvecdeg[CHY] * rvecdeg[CHY] + rvecdeg[CHZ] * rvecdeg[CHZ]);
    fetarad = fetadeg * FPIOVER180;
    fetarad2 = fetarad * fetarad;
 
    // calculate the sine and cosine using small angle approximations or exact
    // angles under sqrt(0.02)=0.141 rad is 8.1 deg and 1620 deg/s (=936deg/s in 3 axes) at 200Hz and 405 deg/s at 50Hz
    if (fetarad2 <= 0.02F)
    {
        // use MacLaurin series up to and including third order
        sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2);
    }
    else if (fetarad2 <= 0.06F)
    {
        // use MacLaurin series up to and including fifth order
        // angles under sqrt(0.06)=0.245 rad is 14.0 deg and 2807 deg/s (=1623deg/s in 3 axes) at 200Hz and 703 deg/s at 50Hz
        fetarad4 = fetarad2 * fetarad2;
        sinhalfeta = fetarad * (0.5F - ONEOVER48 * fetarad2 + ONEOVER3840 * fetarad4);
    }
    else
    {
        // use exact calculation
        sinhalfeta = (float)sinf(0.5F * fetarad);
    }
 
    // compute the vector quaternion components q1, q2, q3
    if (fetadeg != 0.0F)
    {
        // general case with non-zero rotation angle
        ftmp = fscaling * sinhalfeta / fetadeg;
        pq->q1 = rvecdeg[CHX] * ftmp;       // q1 = nx * sin(eta/2)
        pq->q2 = rvecdeg[CHY] * ftmp;       // q2 = ny * sin(eta/2)
        pq->q3 = rvecdeg[CHZ] * ftmp;       // q3 = nz * sin(eta/2)
    }
    else
    {
        // zero rotation angle giving zero vector component
        pq->q1 = pq->q2 = pq->q3 = 0.0F;
    }
 
    // compute the scalar quaternion component q0 by explicit normalization
    // taking care to avoid rounding errors giving negative operand to sqrt
    fvecsq = pq->q1 * pq->q1 + pq->q2 * pq->q2 + pq->q3 * pq->q3;
    if (fvecsq <= 1.0F)
    {
        // normal case
        pq->q0 = sqrtf(1.0F - fvecsq);
    }
    else
    {
        // rounding errors are present
        pq->q0 = 0.0F;
    }
 
    return;
}
 
// compute the orientation quaternion from a 3x3 rotation matrix
void fQuaternionFromRotationMatrix(float R[][3], Quaternion *pq)
{
    float fq0sq;            // q0^2
    float recip4q0;         // 1/4q0
 
    // get q0^2 and q0
    fq0sq = 0.25F * (1.0F + R[CHX][CHX] + R[CHY][CHY] + R[CHZ][CHZ]);
    pq->q0 = sqrtf(fabsf(fq0sq));
 
    // normal case when q0 is not small meaning rotation angle not near 180 deg
    if (pq->q0 > SMALLQ0)
    {
        // calculate q1 to q3
        recip4q0 = 0.25F / pq->q0;
        pq->q1 = recip4q0 * (R[CHY][CHZ] - R[CHZ][CHY]);
        pq->q2 = recip4q0 * (R[CHZ][CHX] - R[CHX][CHZ]);
        pq->q3 = recip4q0 * (R[CHX][CHY] - R[CHY][CHX]);
    } // end of general case
    else
    {
        // special case of near 180 deg corresponds to nearly symmetric matrix
        // which is not numerically well conditioned for division by small q0
        // instead get absolute values of q1 to q3 from leading diagonal
        pq->q1 = sqrtf(fabsf(0.5F + 0.5F * R[CHX][CHX] - fq0sq));
        pq->q2 = sqrtf(fabsf(0.5F + 0.5F * R[CHY][CHY] - fq0sq));
        pq->q3 = sqrtf(fabsf(0.5F + 0.5F * R[CHZ][CHZ] - fq0sq));
 
        // correct the signs of q1 to q3 by examining the signs of differenced off-diagonal terms
        if ((R[CHY][CHZ] - R[CHZ][CHY]) < 0.0F) pq->q1 = -pq->q1;
        if ((R[CHZ][CHX] - R[CHX][CHZ]) < 0.0F) pq->q2 = -pq->q2;
        if ((R[CHX][CHY] - R[CHY][CHX]) < 0.0F) pq->q3 = -pq->q3;
    } // end of special case
 
    // ensure that the resulting quaternion is normalized even if the input rotation matrix was not
    fqAeqNormqA(pq);
 
 
    return;
}
 
// compute the rotation matrix from an orientation quaternion
void fRotationMatrixFromQuaternion(float R[][3], const Quaternion *pq)
{
    float f2q;
    float f2q0q0, f2q0q1, f2q0q2, f2q0q3;
    float f2q1q1, f2q1q2, f2q1q3;
    float f2q2q2, f2q2q3;
    float f2q3q3;
 
    // set f2q to 2*q0 and calculate products
    f2q = 2.0F * pq->q0;
    f2q0q0 = f2q * pq->q0;
    f2q0q1 = f2q * pq->q1;
    f2q0q2 = f2q * pq->q2;
    f2q0q3 = f2q * pq->q3;
    // set f2q to 2*q1 and calculate products
    f2q = 2.0F * pq->q1;
    f2q1q1 = f2q * pq->q1;
    f2q1q2 = f2q * pq->q2;
    f2q1q3 = f2q * pq->q3;
    // set f2q to 2*q2 and calculate products
    f2q = 2.0F * pq->q2;
    f2q2q2 = f2q * pq->q2;
    f2q2q3 = f2q * pq->q3;
    f2q3q3 = 2.0F * pq->q3 * pq->q3;
 
    // calculate the rotation matrix assuming the quaternion is normalized
    R[CHX][CHX] = f2q0q0 + f2q1q1 - 1.0F;
    R[CHX][CHY] = f2q1q2 + f2q0q3;
    R[CHX][CHZ] = f2q1q3 - f2q0q2;
    R[CHY][CHX] = f2q1q2 - f2q0q3;
    R[CHY][CHY] = f2q0q0 + f2q2q2 - 1.0F;
    R[CHY][CHZ] = f2q2q3 + f2q0q1;
    R[CHZ][CHX] = f2q1q3 + f2q0q2;
    R[CHZ][CHY] = f2q2q3 - f2q0q1;
    R[CHZ][CHZ] = f2q0q0 + f2q3q3 - 1.0F;
 
    return;
}
 
// computes rotation vector (deg) from rotation quaternion
void fRotationVectorDegFromQuaternion(Quaternion *pq, float rvecdeg[])
{
    float fetarad;          // rotation angle (rad)
    float fetadeg;          // rotation angle (deg)
    float sinhalfeta;       // sin(eta/2)
    float ftmp;             // scratch variable
 
    // calculate the rotation angle in the range 0 <= eta < 360 deg
    if ((pq->q0 >= 1.0F) || (pq->q0 <= -1.0F))
    {
        // rotation angle is 0 deg or 2*180 deg = 360 deg = 0 deg
        fetarad = 0.0F;
        fetadeg = 0.0F;
    }
    else
    {
        // general case returning 0 < eta < 360 deg
        fetarad = 2.0F * acosf(pq->q0);
        fetadeg = fetarad * F180OVERPI;
    }
 
    // map the rotation angle onto the range -180 deg <= eta < 180 deg
    if (fetadeg >= 180.0F)
    {
        fetadeg -= 360.0F;
        fetarad = fetadeg * FPIOVER180;
    }
 
    // calculate sin(eta/2) which will be in the range -1 to +1
    sinhalfeta = (float)sinf(0.5F * fetarad);
 
    // calculate the rotation vector (deg)
    if (sinhalfeta == 0.0F)
    {
        // the rotation angle eta is zero and the axis is irrelevant
        rvecdeg[CHX] = rvecdeg[CHY] = rvecdeg[CHZ] = 0.0F;
    }
    else
    {
        // general case with non-zero rotation angle
        ftmp = fetadeg / sinhalfeta;
        rvecdeg[CHX] = pq->q1 * ftmp;
        rvecdeg[CHY] = pq->q2 * ftmp;
        rvecdeg[CHZ] = pq->q3 * ftmp;
    }
 
    return;
}
 
// function low pass filters an orientation quaternion and computes virtual gyro rotation rate
void fLPFOrientationQuaternion(Quaternion *pq, Quaternion *pLPq, float flpf, float fdeltat, float fOmega[])
{
    // local variables
    Quaternion fdeltaq;         // delta rotation quaternion
    float rvecdeg[3];                   // rotation vector (deg)
    float ftmp;                         // scratch variable
 
    // set fdeltaq to the delta rotation quaternion conjg(fLPq[n-1) . fqn
    fdeltaq = qconjgAxB(pLPq, pq);
    if (fdeltaq.q0 < 0.0F)
    {
        fdeltaq.q0 = -fdeltaq.q0;
        fdeltaq.q1 = -fdeltaq.q1;
        fdeltaq.q2 = -fdeltaq.q2;
        fdeltaq.q3 = -fdeltaq.q3;
    }
 
    // set ftmp to a scaled value which equals flpf in the limit of small rotations (q0=1)
    // but which rises to 1 (all pass) as the delta rotation angle increases (q0 tends to zero)
    ftmp = flpf + (1.0F - flpf) * sqrtf(fabs(1.0F - fdeltaq.q0 *  fdeltaq.q0));
 
    // scale the vector component of the delta rotation quaternion by the corrected lpf value
    // and re-compute the scalar component q0 to ensure normalization
    fdeltaq.q1 *= ftmp;
    fdeltaq.q2 *= ftmp;
    fdeltaq.q3 *= ftmp;
    fdeltaq.q0 = sqrtf(fabsf(1.0F - fdeltaq.q1 * fdeltaq.q1 - fdeltaq.q2 * fdeltaq.q2 - fdeltaq.q3 * fdeltaq.q3));
 
    // calculate the delta rotation vector from fdeltaq and the virtual gyro angular velocity (deg/s)
    fRotationVectorDegFromQuaternion(&fdeltaq, rvecdeg);
    ftmp = 1.0F / fdeltat;
    fOmega[CHX] = rvecdeg[CHX] * ftmp;
    fOmega[CHY] = rvecdeg[CHY] * ftmp;
    fOmega[CHZ] = rvecdeg[CHZ] * ftmp;
 
    // set LPq[n] = LPq[n-1] . deltaq[n]
    qAeqAxB(pLPq, &fdeltaq);
 
    // renormalize the low pass filtered quaternion to prevent error accumulation.
    // the renormalization function also ensures that q0 is non-negative
    fqAeqNormqA(pLPq);
 
    return;
}
 
// function compute the quaternion product qA * qB
void qAeqBxC(Quaternion *pqA, const Quaternion *pqB, const Quaternion *pqC)
{
    pqA->q0 = pqB->q0 * pqC->q0 - pqB->q1 * pqC->q1 - pqB->q2 * pqC->q2 - pqB->q3 * pqC->q3;
    pqA->q1 = pqB->q0 * pqC->q1 + pqB->q1 * pqC->q0 + pqB->q2 * pqC->q3 - pqB->q3 * pqC->q2;
    pqA->q2 = pqB->q0 * pqC->q2 - pqB->q1 * pqC->q3 + pqB->q2 * pqC->q0 + pqB->q3 * pqC->q1;
    pqA->q3 = pqB->q0 * pqC->q3 + pqB->q1 * pqC->q2 - pqB->q2 * pqC->q1 + pqB->q3 * pqC->q0;
 
    return;
}
 
// function compute the quaternion product qA = qA * qB
void qAeqAxB(Quaternion *pqA, const Quaternion *pqB)
{
    Quaternion qProd;
 
    // perform the quaternion product
    qProd.q0 = pqA->q0 * pqB->q0 - pqA->q1 * pqB->q1 - pqA->q2 * pqB->q2 - pqA->q3 * pqB->q3;
    qProd.q1 = pqA->q0 * pqB->q1 + pqA->q1 * pqB->q0 + pqA->q2 * pqB->q3 - pqA->q3 * pqB->q2;
    qProd.q2 = pqA->q0 * pqB->q2 - pqA->q1 * pqB->q3 + pqA->q2 * pqB->q0 + pqA->q3 * pqB->q1;
    qProd.q3 = pqA->q0 * pqB->q3 + pqA->q1 * pqB->q2 - pqA->q2 * pqB->q1 + pqA->q3 * pqB->q0;
 
    // copy the result back into qA
    *pqA = qProd;
 
    return;
}
 
// function compute the quaternion product conjg(qA) * qB
Quaternion qconjgAxB(const Quaternion *pqA, const Quaternion *pqB)
{
    Quaternion qProd;
 
    qProd.q0 = pqA->q0 * pqB->q0 + pqA->q1 * pqB->q1 + pqA->q2 * pqB->q2 + pqA->q3 * pqB->q3;
    qProd.q1 = pqA->q0 * pqB->q1 - pqA->q1 * pqB->q0 - pqA->q2 * pqB->q3 + pqA->q3 * pqB->q2;
    qProd.q2 = pqA->q0 * pqB->q2 + pqA->q1 * pqB->q3 - pqA->q2 * pqB->q0 - pqA->q3 * pqB->q1;
    qProd.q3 = pqA->q0 * pqB->q3 - pqA->q1 * pqB->q2 + pqA->q2 * pqB->q1 - pqA->q3 * pqB->q0;
 
    return qProd;
}
 
// function normalizes a rotation quaternion and ensures q0 is non-negative
void fqAeqNormqA(Quaternion *pqA)
{
    float fNorm;                    // quaternion Norm
 
    // calculate the quaternion Norm
    fNorm = sqrtf(pqA->q0 * pqA->q0 + pqA->q1 * pqA->q1 + pqA->q2 * pqA->q2 + pqA->q3 * pqA->q3);
    if (fNorm > CORRUPTQUAT)
    {
        // general case
        fNorm = 1.0F / fNorm;
        pqA->q0 *= fNorm;
        pqA->q1 *= fNorm;
        pqA->q2 *= fNorm;
        pqA->q3 *= fNorm;
    }
    else
    {
        // return with identity quaternion since the quaternion is corrupted
        pqA->q0 = 1.0F;
        pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
    }
 
    // correct a negative scalar component if the function was called with negative q0
    if (pqA->q0 < 0.0F)
    {
        pqA->q0 = -pqA->q0;
        pqA->q1 = -pqA->q1;
        pqA->q2 = -pqA->q2;
        pqA->q3 = -pqA->q3;
    }
 
    return;
}
 
// set a quaternion to the unit quaternion
void fqAeq1(Quaternion *pqA)
{
    pqA->q0 = 1.0F;
    pqA->q1 = pqA->q2 = pqA->q3 = 0.0F;
 
    return;
}
 
// function computes the rotation quaternion that rotates unit vector u onto unit vector v as v=q*.u.q
// using q = 1/sqrt(2) * {sqrt(1 + u.v) - u x v / sqrt(1 + u.v)}
void fveqconjgquq(Quaternion *pfq, float fu[], float fv[])
{
    float fuxv[3];              // vector product u x v
    float fsqrt1plusudotv;      // sqrt(1 + u.v)
    float ftmp;                 // scratch
 
    // compute sqrt(1 + u.v) and scalar quaternion component q0 (valid for all angles including 180 deg)
    fsqrt1plusudotv = sqrtf(fabsf(1.0F + fu[CHX] * fv[CHX] + fu[CHY] * fv[CHY] + fu[CHZ] * fv[CHZ]));
    pfq->q0 = ONEOVERSQRT2 * fsqrt1plusudotv;
 
    // calculate the vector product uxv
    fuxv[CHX] = fu[CHY] * fv[CHZ] - fu[CHZ] * fv[CHY];
    fuxv[CHY] = fu[CHZ] * fv[CHX] - fu[CHX] * fv[CHZ];
    fuxv[CHZ] = fu[CHX] * fv[CHY] - fu[CHY] * fv[CHX];
 
    // compute the vector component of the quaternion
    if (fsqrt1plusudotv != 0.0F)
    {
        // general case where u and v are not anti-parallel where u.v=-1
        ftmp = ONEOVERSQRT2 / fsqrt1plusudotv;
        pfq->q1 = -fuxv[CHX] * ftmp;
        pfq->q2 = -fuxv[CHY] * ftmp;
        pfq->q3 = -fuxv[CHZ] * ftmp;
    }
    else
    {
        // degenerate case where u and v are anti-aligned and the 180 deg rotation quaternion is not uniquely defined.
        // first calculate the un-normalized vector component (the scalar component q0 is already set to zero)
        pfq->q1 = fu[CHY] - fu[CHZ];
        pfq->q2 = fu[CHZ] - fu[CHX];
        pfq->q3 = fu[CHX] - fu[CHY];
        // and normalize the quaternion for this case checking for fu[CHX]=fu[CHY]=fuCHZ] where q1=q2=q3=0.
        ftmp = sqrtf(fabsf(pfq->q1 * pfq->q1 + pfq->q2 * pfq->q2 + pfq->q3 * pfq->q3));
        if (ftmp != 0.0F)
        {
            // normal case where all three components of fu (and fv=-fu) are not equal
            ftmp = 1.0F / ftmp;
            pfq->q1 *= ftmp;
            pfq->q2 *= ftmp;
            pfq->q3 *= ftmp;
        }
        else
        {
            // final case where the three entries are equal but since the vectors are known to be normalized
            // the vector u must be 1/root(3)*{1, 1, 1} or -1/root(3)*{1, 1, 1} so simply set the
            // rotation vector to 1/root(2)*{1, -1, 0} to cover both cases
            pfq->q1 = ONEOVERSQRT2;
            pfq->q2 = -ONEOVERSQRT2;
            pfq->q3 = 0.0F;
        }
    }
 
    return;
}