EFM32 Gecko Software Documentation  efm32g-doc-5.1.2
imu.h
Go to the documentation of this file.
1 /***************************************************************************/
16 #ifndef __IMU_H_
17 #define __IMU_H_
18 
19 #include <stdint.h>
20 
21 #include "em_gpio.h"
22 
23 #include "i2cspm.h"
24 
27 
28 /***************************************************************************/
33 /***************************************************************************/
39 #define IMU_OK 0
43 /***************************************************************************/
49 #define IMU_PI 3.14159265358979323
50 #define IMU_DEG_TO_RAD_FACTOR ( (2.0*IMU_PI) / 360.0 )
51 #define IMU_RAD_TO_DEG_FACTOR ( 360.0 / (2.0*IMU_PI) )
52 #define IMU_DEG_TO_RAD(ang) (ang * IMU_DEG_TO_RAD_FACTOR)
53 #define IMU_RAD_TO_DEG(ang) (ang * IMU_RAD_TO_DEG_FACTOR)
55 #define IMU_MAX_ACCEL_FOR_ANGLE 0.9848
57 #define IMU_STATE_DISABLED 0x00
58 #define IMU_STATE_READY 0x01
59 #define IMU_STATE_INITIALIZING 0x02
60 #define IMU_STATE_CALIBRATING 0x03
64 /***************************************************************************/
70 /***************************************************************************/
74 typedef struct _IMU_SensorFusion
75 {
76 
77  /* Direction Cosine Matrix */
78  float dcm[3][3];
80  /* Accelerometer filtering */
81  float aVector[3];
82  float aAccumulator[3];
83  uint32_t aAccumulatorCount;
84  float aSampleRate;
86  /* Gyro */
87  float gVector[3];
88  float gSampleRate;
89  float gDeltaTime;
92  /* Orientation */
93  float angleCorrection[3];
94  float orientation[3];
97 
100 /***************************************************************************/
105 /******************************************************************************/
107 /******************************************************************************/
108 uint32_t IMU_init ( void );
109 uint32_t IMU_deInit ( void );
110 uint8_t IMU_getState ( void );
111 void IMU_update ( void );
112 void IMU_reset ( void );
113 
114 void IMU_accelerationGet ( int16_t avec[3] );
115 void IMU_orientationGet ( int16_t ovec[3] );
116 void IMU_gyroGet ( int16_t gvec[3] );
117 uint32_t IMU_gyroCalibrate ( void );
118 void IMU_getGyroCorrectionAngles ( float acorr[3] );
119 
120 /*****************************************************************************/
122 /*****************************************************************************/
123 void IMU_normalizeAngle ( float *a );
124 void IMU_matrixMultiply ( float c[3][3], float a[3][3], float b[3][3] );
125 void IMU_vectorNormalizeAngle ( float v[3] );
126 void IMU_vectorZero ( float v[3] );
127 void IMU_vectorScale ( float v[3], float scale );
128 void IMU_vectorScalarMultiplication ( float r[3], float v[3], float scale );
129 void IMU_vectorAdd ( float r[3], float a[3], float b[3] );
130 void IMU_vectorSubtract ( float r[3], float a[3], float b[3] );
131 float IMU_vectorDotProduct ( float a[3], float b[3] );
132 void IMU_vectorCrossProduct ( float r[3], float a[3], float b[3] );
133 
134 /*****************************************************************************/
136 /*****************************************************************************/
137 void IMU_dcmReset ( float dcm[3][3] );
138 void IMU_dcmResetZ ( float dcm[3][3] );
139 void IMU_dcmNormalize ( float dcm[3][3] );
140 void IMU_dcmRotate ( float dcm[3][3], float ang[3] );
141 void IMU_dcmGetAngles ( float dcm[3][3], float ang[3] );
142 
143 /*****************************************************************************/
145 /*****************************************************************************/
146 void IMU_config ( float sampleRate );
147 bool IMU_isDataReady ( void );
148 bool IMU_isDataReadyFlag ( void );
149 void IMU_clearDataReadyFlag ( void );
150 
151 void IMU_getAccelerometerData ( float avec[3] );
152 void IMU_getGyroData ( float gvec[3] );
153 
154 /*****************************************************************************/
156 /*****************************************************************************/
158 void IMU_fuseAccelerometerUpdateFilter ( IMU_SensorFusion *f, float avec[3] );
159 
160 void IMU_fuseGyroSetSampleRate ( IMU_SensorFusion *f, float rate );
161 void IMU_fuseGyroUpdate ( IMU_SensorFusion *f, float gvec[3] );
164  bool accValid, bool dirValid,
165  float dirZ );
166 
167 void IMU_fuseNew ( IMU_SensorFusion *f );
168 void IMU_fuseReset ( IMU_SensorFusion *f );
169 void IMU_fuseUpdate ( IMU_SensorFusion *f );
170 
175 #endif /* __IMU_H_ */
Structure to store the sensor fusion data.
Definition: imu.h:74
bool IMU_isDataReadyFlag(void)
Checks if there is new accel/gyro data available for read. In case of ver 0.3.0 and older of the PIC ...
Definition: imu.c:478
void IMU_config(float sampleRate)
Configures the IMU.
Definition: imu.c:177
void IMU_fuseGyroUpdate(IMU_SensorFusion *f, float gvec[3])
Updates the fusion calculation with a new gyro data.
Definition: imu_fuse.c:191
void IMU_normalizeAngle(float *a)
Normalizes the angle ( -PI < angle <= PI )
Definition: imu_math.c:41
void IMU_vectorCrossProduct(float r[3], float a[3], float b[3])
Calculates the cross product of two vectors.
Definition: imu_math.c:293
void IMU_vectorNormalizeAngle(float v[3])
Normalizes the angle of a vector.
Definition: imu_math.c:102
Power and Interrupt Controller Register Definitions.
struct _IMU_SensorFusion IMU_SensorFusion
Structure to store the sensor fusion data.
float gDeltaTime
Definition: imu.h:89
void IMU_dcmReset(float dcm[3][3])
Sets the elements of the DCM matrixto the corresponding elements of the identity matrix.
Definition: imu_dcm.c:44
void IMU_fuseGyroCorrection(IMU_SensorFusion *f, bool accValid, bool dirValid, float dirZ)
Calculates the gyro correction vector.
Definition: imu_fuse.c:231
uint8_t IMU_getState(void)
Returns IMU state.
Definition: imu.c:162
void IMU_vectorScale(float v[3], float scale)
Scales a vector by a factor.
Definition: imu_math.c:179
void IMU_dcmNormalize(float dcm[3][3])
Normalizes the DCM matrix.
Definition: imu_dcm.c:137
float gDeltaTimeScale
Definition: imu.h:90
void IMU_getGyroData(float gvec[3])
Retrieves the raw gyroscope data from the IMU.
Definition: imu.c:425
void IMU_fuseAccelerometerUpdateFilter(IMU_SensorFusion *f, float avec[3])
The current accelerometer data is added to the accumulator.
Definition: imu_fuse.c:146
void IMU_vectorZero(float v[3])
Sets all elements of a vector to 0.
Definition: imu_math.c:125
void IMU_fuseAccelerometerSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the accelerometer sample rate in the IMU_SensorFusion structure.
Definition: imu_fuse.c:124
RGB LED profiles.
void IMU_update(void)
Gets a new set of data from the accel and gyro sensor and updates the fusion calculation.
Definition: imu.c:343
void IMU_matrixMultiply(float c[3][3], float a[3][3], float b[3][3])
Multiplies two 3x3 matrices.
Definition: imu_math.c:72
void IMU_dcmRotate(float dcm[3][3], float ang[3])
Rotates the DCM matrix by a given angle.
Definition: imu_dcm.c:96
General Purpose IO (GPIO) peripheral API.
I2C simple poll-based master mode driver for the DK/STK.
float gSampleRate
Definition: imu.h:88
void IMU_getAccelerometerData(float avec[3])
Retrieves the raw acceleration data from the IMU.
Definition: imu.c:378
uint32_t IMU_gyroCalibrate(void)
Performs gyroscope calibration to cancel gyro bias.
Definition: imu.c:315
void IMU_gyroGet(int16_t gvec[3])
Retrieves the processed gyroscope data.
Definition: imu.c:297
void IMU_fuseReset(IMU_SensorFusion *f)
Clears the values of the sensor fusion object.
Definition: imu_fuse.c:327
uint32_t IMU_init(void)
Initializes and calibrates the IMU.
Definition: imu.c:82
void IMU_dcmGetAngles(float dcm[3][3], float ang[3])
Calculates the Euler angles (roll, pitch, yaw) from the DCM matrix.
Definition: imu_dcm.c:184
void IMU_clearDataReadyFlag(void)
Clears the IMU data ready flag. In case of ver 0.3.0 and older of the PIC firmware the interrupt regi...
Definition: imu.c:510
void IMU_fuseGyroCorrectionClear(IMU_SensorFusion *f)
Clears the gyro correction vector.
Definition: imu_fuse.c:169
void IMU_getGyroCorrectionAngles(float acorr[3])
Retrieves the processed gyroscope correction angles.
Definition: imu.c:404
void IMU_dcmResetZ(float dcm[3][3])
DCM reset, Z direction.
Definition: imu_dcm.c:69
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
Definition: imu_math.c:208
void IMU_accelerationGet(int16_t avec[3])
Retrieves the processed acceleration data.
Definition: imu.c:245
bool IMU_isDataReady(void)
Checks if there is new accel/gyro data available for read.
Definition: imu.c:448
void IMU_fuseNew(IMU_SensorFusion *f)
Initializes a new IMU_SensorFusion structure.
Definition: imu_fuse.c:51
void IMU_vectorSubtract(float r[3], float a[3], float b[3])
Subtracts vector b from vector a.
Definition: imu_math.c:237
float IMU_vectorDotProduct(float a[3], float b[3])
Calculates the dot product of two vectors.
Definition: imu_math.c:262
void IMU_fuseUpdate(IMU_SensorFusion *f)
Updates the fusion calculation.
Definition: imu_fuse.c:353
uint32_t aAccumulatorCount
Definition: imu.h:83
float aSampleRate
Definition: imu.h:84
void IMU_fuseGyroSetSampleRate(IMU_SensorFusion *f, float rate)
Sets the gyro sample rate and related values in the IMU_SensorFusion structure.
Definition: imu_fuse.c:100
void IMU_vectorScalarMultiplication(float r[3], float v[3], float scale)
Multiplies a vector by a scalar.
Definition: imu_math.c:153
void IMU_reset(void)
Resets the fusion calculation.
Definition: imu.c:359
void IMU_orientationGet(int16_t ovec[3])
Retrieves the processed orientation data.
Definition: imu.c:276
uint32_t IMU_deInit(void)
De-initializes the IMU chip.
Definition: imu.c:143