EFM32 Happy Gecko Software Documentation  efm32hg-doc-5.1.2
imu_dcm.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 /***************************************************************************/
28 /***************************************************************************/
33 /***************************************************************************/
44 void IMU_dcmReset( float dcm[3][3] )
45 {
46 
47  int x, y;
48 
49  for( y = 0; y < 3; y++ ) {
50  for( x = 0; x < 3; x++ ) {
51  dcm[y][x] = ( x == y ) ? 1 : 0;
52  }
53  }
54 
55  return;
56 
57 }
58 
59 /***************************************************************************/
69 void IMU_dcmResetZ( float dcm[3][3] )
70 {
71 
72  dcm[0][0] = 1;
73  dcm[0][1] = 0;
74  dcm[0][2] = 0;
75  IMU_vectorCrossProduct( &dcm[1][0], &dcm[0][0], &dcm[2][0] );
76  IMU_vectorScale ( &dcm[1][0], -1.0f );
77  IMU_vectorCrossProduct( &dcm[0][0], &dcm[1][0], &dcm[2][0] );
78 
79  return;
80 
81 }
82 
83 /***************************************************************************/
96 void IMU_dcmRotate( float dcm[3][3], float angle[3] )
97 {
98 
99  int x, y;
100  float um[3][3];
101  float tm[3][3];
102 
103  um[0][0] = 0.0f;
104  um[0][1] = -angle[2];
105  um[0][2] = angle[1];
106 
107  um[1][0] = angle[2];
108  um[1][1] = 0.0f;
109  um[1][2] = -angle[0];
110 
111  um[2][0] = -angle[1];
112  um[2][1] = angle[0];
113  um[2][2] = 0.0f;
114 
115  IMU_matrixMultiply( tm, dcm, um );
116 
117  for( y = 0; y < 3; y++ ) {
118  for( x = 0; x < 3; x++ ) {
119  dcm[y][x] += tm[y][x];
120  }
121  }
122 
123  return;
124 
125 }
126 
127 /***************************************************************************/
137 void IMU_dcmNormalize( float dcm[3][3] )
138 {
139 
140  float error;
141  float renorm;
142  float r;
143  float temporary[3][3];
144 
145  error = -0.5f * IMU_vectorDotProduct( &dcm[0][0], &dcm[1][0] );
146 
147  IMU_vectorScalarMultiplication( &temporary[0][0], &dcm[1][0], error );
148  IMU_vectorScalarMultiplication( &temporary[1][0], &dcm[0][0], error );
149 
150  IMU_vectorAdd( &temporary[0][0], &temporary[0][0], &dcm[0][0] );
151  IMU_vectorAdd( &temporary[1][0], &temporary[1][0], &dcm[1][0] );
152 
153  IMU_vectorCrossProduct( &temporary[2][0], &temporary[0][0], &temporary[1][0] );
154 
155  r = IMU_vectorDotProduct( &temporary[0][0], &temporary[0][0] );
156  renorm = 0.5f * ( 3.0f - r );
157  IMU_vectorScalarMultiplication( &dcm[0][0], &temporary[0][0], renorm );
158 
159  r = IMU_vectorDotProduct( &temporary[1][0], &temporary[1][0] );
160  renorm = 0.5f * ( 3.0f - r );
161  IMU_vectorScalarMultiplication( &dcm[1][0], &temporary[1][0], renorm );
162 
163  r = IMU_vectorDotProduct( &temporary[2][0], &temporary[2][0] );
164  renorm = 0.5f * ( 3.0f - r );
165  IMU_vectorScalarMultiplication( &dcm[2][0], &temporary[2][0], renorm );
166 
167  return;
168 
169 }
170 
171 /***************************************************************************/
184 void IMU_dcmGetAngles( float dcm[3][3], float angle[3] )
185 {
186 
187  /* Roll */
188  angle[0] = atan2f( dcm[2][1], dcm[2][2] );
189 
190  /* Pitch */
191  angle[1] = -asinf( dcm[2][0] );
192 
193  /* Yaw */
194  angle[2] = atan2f( dcm[1][0], dcm[0][0] );
195 
196  return;
197 
198 }
199 
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_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_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
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
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_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
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