imu6d.h

#ifndef _AVRIO_IMU6D_H_
#define _AVRIO_IMU6D_H_

#include <avrio/defs.h>

__BEGIN_C_DECLS
/* ========================================================================== */
#include <avrio/vector3f.h>
#include <avrio/quaternionf.h>

/* structures =============================================================== */
typedef struct xImu6dFilter {

    // Quatfernion orientation of earth frame relative to auxiliary frame.
    xQuatf xAEq;

    // Estimated orientation quaternionf elements with initial conditions.
    xQuatf xSEq;

    // Attitude Vector (Euler Angles)
    float xAtt[3];

    // Sampling period
    float fDeltaT;

    // Gyroscope measurement error (in degrees per second).
    float fGyroMeasError;

    // Compute fBeta (filter tuning constant...)
    float fBeta;

    // Initializer boolean
    bool bFirstUpdate;

} xImu6dFilter;


/* internal public functions ================================================ */
void vImu6dInit (xImu6dFilter * xFilter, float fFilterRate, float fGyroError);

void vImu6dUpdateFilter(xImu6dFilter * xFilter, float * xAcc, float * xRate);

void vImu6dComputeEuler(xImu6dFilter * xFilter);

void vImu6dReset(xImu6dFilter * xFilter);

#  if defined(__DOXYGEN__)
/* 
 * __DOXYGEN__ defined
 * Partie documentation ne devant pas être compilée.
 * =============================================================================
 */
float fImu6dRoll(xImu6dFilter * xFilter);

float fImu6dPitch(xImu6dFilter * xFilter);

float fImu6dYaw(xImu6dFilter * xFilter);

#  else
/* 
 * __DOXYGEN__ not defined
 * Partie ne devant pas être documentée.
 * =============================================================================
 */

// -----------------------------------------------------------------------------
__STATIC_ALWAYS_INLINE (float 
fImu6dRoll(xImu6dFilter * xFilter)) {

    return xFilter->xAtt[0];
}

// -----------------------------------------------------------------------------
__STATIC_ALWAYS_INLINE (float 
fImu6dPitch(xImu6dFilter * xFilter)) {

    return xFilter->xAtt[1];
}

// -----------------------------------------------------------------------------
__STATIC_ALWAYS_INLINE (float 
fImu6dYaw(xImu6dFilter * xFilter)) {

    return xFilter->xAtt[2];
}

/* public variables ========================================================= */

#  endif /* __DOXYGEN__ not defined */
/* ========================================================================== */
__END_C_DECLS
#endif  /* _AVRIO_IMU6D_H_ not defined */