imu_math.c

Go to the documentation of this file.

/***************************************************************************/
#include <stdint.h>
#include <stdbool.h>

#include "thunderboard/imu/imu.h"

/***************************************************************************/
/***************************************************************************/
/***************************************************************************/
void IMU_normalizeAngle( float *a )
{

   while( *a >= IMU_PI ) {
      *a -= 2 * IMU_PI;
   }

   while( *a < -IMU_PI ) {
      *a += 2 * IMU_PI;
   }

   return;

}

/***************************************************************************/
void IMU_matrixMultiply( float c[3][3], float a[3][3], float b[3][3] )
{

   int x, y, w;
   float op[3];

   for( x = 0; x < 3; x++ ) {
      for( y = 0; y < 3; y++ ) {
         for( w = 0; w < 3; w++ ) {
            op[w] = a[x][w] * b[w][y];
         }
         c[x][y] = op[0] + op[1] + op[2];

      }
   }

   return;

}

/***************************************************************************/
void IMU_vectorNormalizeAngle( float v[3] )
{

   int n;

   for( n = 0; n < 3; n++ ) {
      IMU_normalizeAngle( &v[n] );
   }

   return;

}

/***************************************************************************/
void IMU_vectorZero( float v[3] )
{
   int n;

   for( n = 0; n < 3; n++ ) {
      v[n] = 0.0f;
   }

   return;

}

/***************************************************************************/
void IMU_vectorScalarMultiplication( float r[3], float v[3], float scale )
{

   int n;

   for( n = 0; n < 3; n++ ) {
      r[n] = v[n] * scale;
   }

   return;

}

/***************************************************************************/
void IMU_vectorScale( float v[3], float scale )
{

   int n;

   for( n = 0; n < 3; n++ ) {
      v[n] *= scale;
   }

   return;

}

/***************************************************************************/
void IMU_vectorAdd( float r[3], float a[3], float b[3] )
{

   int n;

   for( n = 0; n < 3; n++ ) {
      r[n] = a[n] + b[n];
   }

   return;

}

/***************************************************************************/
void IMU_vectorSubtract( float r[3], float a[3], float b[3] )
{
   int n;

   for( n = 0; n < 3; n++ ) {
      r[n] = a[n] - b[n];
   }

   return;

}

/***************************************************************************/
float IMU_vectorDotProduct( float a[3], float b[3] )
{

   float r;
   int n;

   r = 0.0f;
   for( n = 0; n < 3; n++ ) {
      r += a[n] * b[n];
   }

   return r;

}

/***************************************************************************/
void IMU_vectorCrossProduct( float r[3], float a[3], float b[3] )
{

   r[0] = a[1] * b[2] - a[2] * b[1];
   r[1] = a[2] * b[0] - a[0] * b[2];
   r[2] = a[0] * b[1] - a[1] * b[0];

   return;

}