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];
83 c[x][y] = op[0] + op[1] + op[2];
107 for( n = 0; n < 3; n++ ) {
129 for( n = 0; n < 3; n++ ) {
158 for( n = 0; n < 3; n++ ) {
184 for( n = 0; n < 3; n++ ) {
213 for( n = 0; n < 3; n++ ) {
241 for( n = 0; n < 3; n++ ) {
269 for( n = 0; n < 3; n++ ) {
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];
void IMU_normalizeAngle(float *a)
Normalizes the angle ( -PI < angle <= PI )
void IMU_vectorCrossProduct(float r[3], float a[3], float b[3])
Calculates the cross product of two vectors.
void IMU_vectorNormalizeAngle(float v[3])
Normalizes the angle of a vector.
void IMU_vectorScale(float v[3], float scale)
Scales a vector by a factor.
void IMU_vectorZero(float v[3])
Sets all elements of a vector to 0.
void IMU_matrixMultiply(float c[3][3], float a[3][3], float b[3][3])
Multiplies two 3x3 matrices.
Inertial Measurement Unit DCM matrix related routines.
void IMU_vectorAdd(float r[3], float a[3], float b[3])
Adds two vectors.
void IMU_vectorSubtract(float r[3], float a[3], float b[3])
Subtracts vector b from vector a.
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.