imu.h

Go to the documentation of this file.

/***************************************************************************/
#ifndef __IMU_H_
#define __IMU_H_

#include <stdint.h>

#include "em_gpio.h"

#include "i2cspm.h"

#include "thunderboard/board_pic_regs.h"
#include "thunderboard/board_rgbled_profiles.h"

/***************************************************************************/
/***************************************************************************/
#define IMU_OK                         0  
/***************************************************************************/
#define IMU_PI                     3.14159265358979323            
#define IMU_DEG_TO_RAD_FACTOR      ( (2.0*IMU_PI) / 360.0 )       
#define IMU_RAD_TO_DEG_FACTOR      ( 360.0 / (2.0*IMU_PI) )       
#define IMU_DEG_TO_RAD(ang)        (ang * IMU_DEG_TO_RAD_FACTOR)  
#define IMU_RAD_TO_DEG(ang)        (ang * IMU_RAD_TO_DEG_FACTOR)  
#define IMU_MAX_ACCEL_FOR_ANGLE    0.9848  
#define IMU_STATE_DISABLED         0x00    
#define IMU_STATE_READY            0x01    
#define IMU_STATE_INITIALIZING     0x02    
#define IMU_STATE_CALIBRATING      0x03    
/***************************************************************************/
/***************************************************************************/
typedef struct _IMU_SensorFusion
{

   /* Direction Cosine Matrix */
   float     dcm[3][3];          
   /* Accelerometer filtering */
   float     aVector[3];         
   float     aAccumulator[3];    
   uint32_t  aAccumulatorCount;  
   float     aSampleRate;        
   /* Gyro */
   float     gVector[3];         
   float     gSampleRate;        
   float     gDeltaTime;         
   float     gDeltaTimeScale;    
   /* Orientation */
   float     angleCorrection[3]; 
   float     orientation[3];     
} IMU_SensorFusion;

/***************************************************************************/
/******************************************************************************/
/******************************************************************************/
uint32_t IMU_init                       ( void );
uint32_t IMU_deInit                     ( void );
uint8_t  IMU_getState                   ( void );
void     IMU_update                     ( void );
void     IMU_reset                      ( void );

void     IMU_accelerationGet            ( int16_t avec[3] );
void     IMU_orientationGet             ( int16_t ovec[3] );
void     IMU_gyroGet                    ( int16_t gvec[3] );
uint32_t IMU_gyroCalibrate              ( void );
void     IMU_getGyroCorrectionAngles    ( float acorr[3] );

/*****************************************************************************/
/*****************************************************************************/
void     IMU_normalizeAngle             ( float *a );
void     IMU_matrixMultiply             ( float c[3][3], float a[3][3], float b[3][3] );
void     IMU_vectorNormalizeAngle       ( float v[3] );
void     IMU_vectorZero                 ( float v[3] );
void     IMU_vectorScale                ( float v[3], float scale );
void     IMU_vectorScalarMultiplication ( float r[3], float v[3], float scale );
void     IMU_vectorAdd                  ( float r[3], float a[3], float b[3] );
void     IMU_vectorSubtract             ( float r[3], float a[3], float b[3] );
float    IMU_vectorDotProduct           ( float a[3], float b[3] );
void     IMU_vectorCrossProduct         ( float r[3], float a[3], float b[3] );

/*****************************************************************************/
/*****************************************************************************/
void     IMU_dcmReset                   ( float dcm[3][3] );
void     IMU_dcmResetZ                  ( float dcm[3][3] );
void     IMU_dcmNormalize               ( float dcm[3][3] );
void     IMU_dcmRotate                  ( float dcm[3][3], float ang[3] );
void     IMU_dcmGetAngles               ( float dcm[3][3], float ang[3] );

/*****************************************************************************/
/*****************************************************************************/
void     IMU_config                     ( float sampleRate );
bool     IMU_isDataReady                ( void );
bool     IMU_isDataReadyFlag            ( void );
void     IMU_clearDataReadyFlag         ( void );

void     IMU_getAccelerometerData       ( float avec[3] );
void     IMU_getGyroData                ( float gvec[3] );

/*****************************************************************************/
/*****************************************************************************/
void     IMU_fuseAccelerometerSetSampleRate ( IMU_SensorFusion *f, float rate );
void     IMU_fuseAccelerometerUpdateFilter  ( IMU_SensorFusion *f, float avec[3] );

void     IMU_fuseGyroSetSampleRate          ( IMU_SensorFusion *f, float rate );
void     IMU_fuseGyroUpdate                 ( IMU_SensorFusion *f, float gvec[3] );
void     IMU_fuseGyroCorrectionClear        ( IMU_SensorFusion *f );
void     IMU_fuseGyroCorrection             ( IMU_SensorFusion *f,
                                              bool accValid, bool dirValid,
                                              float dirZ );

void     IMU_fuseNew                        ( IMU_SensorFusion *f );
void     IMU_fuseReset                      ( IMU_SensorFusion *f );
void     IMU_fuseUpdate                     ( IMU_SensorFusion *f );

#endif /* __IMU_H_ */