EFM32 Wonder Gecko Software Documentation  efm32wg-doc-5.1.2
imu_fuse.c
Go to the documentation of this file.
1 /***************************************************************************/
16 #include <stdint.h>
17 #include <stdbool.h>
18 
19 #include <math.h>
20 
21 #include "thunderboard/imu/imu.h"
22 
23 extern uint8_t IMU_state;
24 
25 /***************************************************************************/
30 /***************************************************************************/
37 static bool IMU_isAccelerationOK( IMU_SensorFusion *f );
38 
41 /***************************************************************************/
52 {
53 
54  f->aVector[0] = 0.0f;
55  f->aVector[1] = 0.0f;
56  f->aVector[2] = 0.0f;
57 
58  f->aAccumulator[0] = 0.0f;
59  f->aAccumulator[1] = 0.0f;
60  f->aAccumulator[2] = 0.0f;
61  f->aAccumulatorCount = 0;
62  f->aSampleRate = 0.0f;
63 
64  f->angleCorrection[0] = 0.0f;
65  f->angleCorrection[1] = 0.0f;
66  f->angleCorrection[2] = 0.0f;
67 
68  f->dcm[0][0] = 0.0f; f->dcm[0][1] = 0.0f; f->dcm[0][2] = 0.0f;
69  f->dcm[1][0] = 0.0f; f->dcm[1][1] = 0.0f; f->dcm[1][2] = 0.0f;
70  f->dcm[2][0] = 0.0f; f->dcm[2][1] = 0.0f; f->dcm[2][2] = 0.0f;
71 
72  f->gVector[0] = 0.0f;
73  f->gVector[1] = 0.0f;
74  f->gVector[2] = 0.0f;
75  f->gSampleRate = 0.0f;
76  f->gDeltaTime = 0.0f;
77 
78  f->orientation[0] = 0.0f;
79  f->orientation[1] = 0.0f;
80  f->orientation[2] = 0.0f;
81 
82  return;
83 
84 }
85 
86 /***************************************************************************/
101 {
102 
103  f->gSampleRate = rate;
104  f->gDeltaTime = 1.0f / rate;
106 
107  return;
108 
109 }
110 
111 /***************************************************************************/
125 {
126 
127  f->gSampleRate = rate;
128 
129  return;
130 
131 }
132 
133 /***************************************************************************/
147 {
148 
149  f->aAccumulator[0] += avec[0];
150  f->aAccumulator[1] += avec[1];
151  f->aAccumulator[2] += avec[2];
152 
153  f->aAccumulatorCount++;
154 
155  return;
156 
157 }
158 
159 /***************************************************************************/
170 {
171 
173 
174  return;
175 
176 }
177 
178 /***************************************************************************/
191 void IMU_fuseGyroUpdate( IMU_SensorFusion *f, float gvec[3] )
192 {
193 
194  float dgvec[3];
195  float rgvec[3];
196 
197  /* Calculate 3D rotation over delta-T */
199 
200  /* Add delta-t rotation to fusion correction angle */
201  IMU_vectorAdd( rgvec, dgvec, f->angleCorrection );
202 
203  /* DCM rotation */
204  IMU_dcmRotate ( f->dcm, rgvec );
205  IMU_dcmNormalize( f->dcm );
206  IMU_dcmGetAngles( f->dcm, f->orientation );
207 
208  return;
209 
210 }
211 
212 /***************************************************************************/
231 void IMU_fuseGyroCorrection( IMU_SensorFusion *f, bool accValid, bool dirValid, float dirZ )
232 {
233 
234  float accAngle[3];
235  float accx, accy, accz;
236 
237  /* Acceleration components */
238  accx = -f->aVector[0];
239  accy = -f->aVector[1];
240  accz = f->aVector[2];
241 
242  /* Clear correction angles */
244 
245  if( ( accValid == true ) && IMU_isAccelerationOK( f ) ) {
246  if( accz >= 0 ) {
247 
248  accAngle[0] = asinf( accy );
249  accAngle[1] = -asinf( accx );
250  accAngle[2] = dirZ;
251 
252  IMU_vectorSubtract( f->angleCorrection, accAngle, f->orientation );
254 
255  }
256  else {
257 
258  accAngle[0] = IMU_PI - asinf( accy );
259  accAngle[1] = -asinf( accx );
260  accAngle[2] = IMU_PI + dirZ;
261 
262  IMU_vectorNormalizeAngle( accAngle );
263  IMU_vectorSubtract( f->angleCorrection, accAngle, f->orientation );
265 
266  f->angleCorrection[1] = -f->angleCorrection[1];
267 
268  }
269 
270  if( dirValid == false ) {
271  f->angleCorrection[2] = 0;
272  }
273 
274  IMU_vectorScale( f->angleCorrection, 0.5f / (float) f->gSampleRate );
275 
276  }
277 
278  return;
279 
280 }
281 
284 /***************************************************************************/
294 static bool IMU_isAccelerationOK( IMU_SensorFusion *f )
295 {
296 
297  float accx;
298  float accy;
299  bool r;
300 
301  accx = f->aVector[0];
302  accy = f->aVector[1];
303  if( ( accx >= -IMU_MAX_ACCEL_FOR_ANGLE ) && ( accx <= IMU_MAX_ACCEL_FOR_ANGLE ) &&
304  ( accy >= -IMU_MAX_ACCEL_FOR_ANGLE ) && ( accy <= IMU_MAX_ACCEL_FOR_ANGLE ) ) {
305  r = true;
306  }
307  else {
308  r = false;
309  }
310 
311  return r;
312 
313 }
314 
317 /***************************************************************************/
328 {
329 
330  IMU_vectorZero( f->aVector );
332  f->aAccumulatorCount = 0;
333  IMU_vectorZero( f->gVector );
336 
337  IMU_dcmReset( f->dcm );
338 
339  return;
340 
341 }
342 
343 /***************************************************************************/
354 {
355 
356  if( IMU_state != IMU_STATE_READY ){
357  return;
358  }
359 
360  /* Get accelerometer data and update Fuse filter */
363 
364  /* Get gyro data and update fuse */
365  IMU_getGyroData( f->gVector );
366  f->gVector[0] = -f->gVector[0];
367  f->gVector[1] = -f->gVector[1];
368  IMU_fuseGyroUpdate( f, f->gVector );
369 
370  /* Perform fusion to compensate for gyro drift */
371  IMU_fuseGyroCorrection( f, true, false, 0 );
372 
373  return;
374 
375 }
376 
float aVector[3]
Definition: imu.h:81
Structure to store the sensor fusion data.
Definition: imu.h:74
void IMU_fuseGyroUpdate(IMU_SensorFusion *f, float gvec[3])
Updates the fusion calculation with a new gyro data.
Definition: imu_fuse.c:191
float dcm[3][3]
Definition: imu.h:78
#define IMU_STATE_READY
Definition: imu.h:58
#define IMU_PI
Definition: imu.h:49
void IMU_vectorNormalizeAngle(float v[3])
Normalizes the angle of a vector.
Definition: imu_math.c:102
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
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
#define IMU_MAX_ACCEL_FOR_ANGLE
Definition: imu.h:55
float angleCorrection[3]
Definition: imu.h:93
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
void IMU_dcmRotate(float dcm[3][3], float ang[3])
Rotates the DCM matrix by a given angle.
Definition: imu_dcm.c:96
float gSampleRate
Definition: imu.h:88
void IMU_getAccelerometerData(float avec[3])
Retrieves the raw acceleration data from the IMU.
Definition: imu.c:378
void IMU_fuseReset(IMU_SensorFusion *f)
Clears the values of the sensor fusion object.
Definition: imu_fuse.c:327
Inertial Measurement Unit DCM matrix related routines.
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_fuseGyroCorrectionClear(IMU_SensorFusion *f)
Clears the gyro correction vector.
Definition: imu_fuse.c:169
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
Definition: imu_math.c:208
void IMU_fuseNew(IMU_SensorFusion *f)
Initializes a new IMU_SensorFusion structure.
Definition: imu_fuse.c:51
uint8_t IMU_state
Definition: imu.c:41
float aAccumulator[3]
Definition: imu.h:82
void IMU_vectorSubtract(float r[3], float a[3], float b[3])
Subtracts vector b from vector a.
Definition: imu_math.c:237
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
#define IMU_DEG_TO_RAD_FACTOR
Definition: imu.h:50
void IMU_vectorScalarMultiplication(float r[3], float v[3], float scale)
Multiplies a vector by a scalar.
Definition: imu_math.c:153
float orientation[3]
Definition: imu.h:94
float gVector[3]
Definition: imu.h:87