EFM32 Gecko Software Documentation  efm32g-doc-5.1.2
imu_math.c
Go to the documentation of this file.
1 /***************************************************************************/
16 #include <stdint.h>
17 #include <stdbool.h>
18 
19 #include "thunderboard/imu/imu.h"
20 
21 /***************************************************************************/
26 /***************************************************************************/
31 /***************************************************************************/
41 void IMU_normalizeAngle( float *a )
42 {
43 
44  while( *a >= IMU_PI ) {
45  *a -= 2 * IMU_PI;
46  }
47 
48  while( *a < -IMU_PI ) {
49  *a += 2 * IMU_PI;
50  }
51 
52  return;
53 
54 }
55 
56 /***************************************************************************/
72 void IMU_matrixMultiply( float c[3][3], float a[3][3], float b[3][3] )
73 {
74 
75  int x, y, w;
76  float op[3];
77 
78  for( x = 0; x < 3; x++ ) {
79  for( y = 0; y < 3; y++ ) {
80  for( w = 0; w < 3; w++ ) {
81  op[w] = a[x][w] * b[w][y];
82  }
83  c[x][y] = op[0] + op[1] + op[2];
84 
85  }
86  }
87 
88  return;
89 
90 }
91 
92 /***************************************************************************/
102 void IMU_vectorNormalizeAngle( float v[3] )
103 {
104 
105  int n;
106 
107  for( n = 0; n < 3; n++ ) {
108  IMU_normalizeAngle( &v[n] );
109  }
110 
111  return;
112 
113 }
114 
115 /***************************************************************************/
125 void IMU_vectorZero( float v[3] )
126 {
127  int n;
128 
129  for( n = 0; n < 3; n++ ) {
130  v[n] = 0.0f;
131  }
132 
133  return;
134 
135 }
136 
137 /***************************************************************************/
153 void IMU_vectorScalarMultiplication( float r[3], float v[3], float scale )
154 {
155 
156  int n;
157 
158  for( n = 0; n < 3; n++ ) {
159  r[n] = v[n] * scale;
160  }
161 
162  return;
163 
164 }
165 
166 /***************************************************************************/
179 void IMU_vectorScale( float v[3], float scale )
180 {
181 
182  int n;
183 
184  for( n = 0; n < 3; n++ ) {
185  v[n] *= scale;
186  }
187 
188  return;
189 
190 }
191 
192 /***************************************************************************/
208 void IMU_vectorAdd( float r[3], float a[3], float b[3] )
209 {
210 
211  int n;
212 
213  for( n = 0; n < 3; n++ ) {
214  r[n] = a[n] + b[n];
215  }
216 
217  return;
218 
219 }
220 
221 /***************************************************************************/
237 void IMU_vectorSubtract( float r[3], float a[3], float b[3] )
238 {
239  int n;
240 
241  for( n = 0; n < 3; n++ ) {
242  r[n] = a[n] - b[n];
243  }
244 
245  return;
246 
247 }
248 
249 /***************************************************************************/
262 float IMU_vectorDotProduct( float a[3], float b[3] )
263 {
264 
265  float r;
266  int n;
267 
268  r = 0.0f;
269  for( n = 0; n < 3; n++ ) {
270  r += a[n] * b[n];
271  }
272 
273  return r;
274 
275 }
276 
277 /***************************************************************************/
293 void IMU_vectorCrossProduct( float r[3], float a[3], float b[3] )
294 {
295 
296  r[0] = a[1] * b[2] - a[2] * b[1];
297  r[1] = a[2] * b[0] - a[0] * b[2];
298  r[2] = a[0] * b[1] - a[1] * b[0];
299 
300  return;
301 
302 }
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
#define IMU_PI
Definition: imu.h:49
void IMU_vectorNormalizeAngle(float v[3])
Normalizes the angle of a vector.
Definition: imu_math.c:102
void IMU_vectorScale(float v[3], float scale)
Scales a vector by a factor.
Definition: imu_math.c:179
void IMU_vectorZero(float v[3])
Sets all elements of a vector to 0.
Definition: imu_math.c:125
void IMU_matrixMultiply(float c[3][3], float a[3][3], float b[3][3])
Multiplies two 3x3 matrices.
Definition: imu_math.c:72
Inertial Measurement Unit DCM matrix related routines.
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
Definition: imu_math.c:208
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_vectorScalarMultiplication(float r[3], float v[3], float scale)
Multiplies a vector by a scalar.
Definition: imu_math.c:153