49 for( y = 0; y < 3; y++ ) {
50 for( x = 0; x < 3; x++ ) {
51 dcm[y][x] = ( x == y ) ? 1 : 0;
104 um[0][1] = -angle[2];
109 um[1][2] = -angle[0];
111 um[2][0] = -angle[1];
117 for( y = 0; y < 3; y++ ) {
118 for( x = 0; x < 3; x++ ) {
119 dcm[y][x] += tm[y][x];
143 float temporary[3][3];
150 IMU_vectorAdd( &temporary[0][0], &temporary[0][0], &dcm[0][0] );
151 IMU_vectorAdd( &temporary[1][0], &temporary[1][0], &dcm[1][0] );
156 renorm = 0.5f * ( 3.0f - r );
160 renorm = 0.5f * ( 3.0f - r );
164 renorm = 0.5f * ( 3.0f - r );
188 angle[0] = atan2f( dcm[2][1], dcm[2][2] );
191 angle[1] = -asinf( dcm[2][0] );
194 angle[2] = atan2f( dcm[1][0], dcm[0][0] );
void IMU_vectorCrossProduct(float r[3], float a[3], float b[3])
Calculates the cross product of two vectors.
void IMU_dcmReset(float dcm[3][3])
Sets the elements of the DCM matrixto the corresponding elements of the identity matrix.
void IMU_vectorScale(float v[3], float scale)
Scales a vector by a factor.
void IMU_dcmNormalize(float dcm[3][3])
Normalizes the DCM matrix.
void IMU_matrixMultiply(float c[3][3], float a[3][3], float b[3][3])
Multiplies two 3x3 matrices.
void IMU_dcmRotate(float dcm[3][3], float ang[3])
Rotates the DCM matrix by a given angle.
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.
void IMU_dcmResetZ(float dcm[3][3])
DCM reset, Z direction.
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
float IMU_vectorDotProduct(float a[3], float b[3])
Calculates the dot product of two vectors.
void IMU_vectorScalarMultiplication(float r[3], float v[3], float scale)
Multiplies a vector by a scalar.