File content as of revision 2:503a5ac6c3b6:

#include "mbed.h"
#include "math.h"
#include "L3GD20.h"
#include "LSM303DLHC.h"

L3GD20 gyro(PTE25, PTE24);
LSM303DLHC compass(PTE25, PTE24);

#define XBIAS 0
#define YBIAS 0
#define ZBIAS 0

#define M_PI            3.1415926535897932384626433832795f
#define TWO_PI          6.283185307179586476925286766559f

#define RADTODEGREE     57.295779513082320876798154814105f

#define GRAVITY       1.0f

#define THRESHOLD 1.1f

Timer t1;

float vC[3], cC[3], magnetom[3], inertialAccel[3];

float aP[3], vP[3], cP[3], deltaT, timeStamp, degGyro[3], angPos[3], prevAngPos[3];

double roll = 0, pitch = 0, yaw = 0;

float magCheck;

float accel[3], bias[3], magnDiff;

int printStamp = 0, stopStamp = 0, dangerAhead = 0, howLong = 0, count[3], pos[5][3]= { {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0}, {0,0,0} };

int flag = 0, signBit = 0;

/********************************************************************************************************************************/
//                                          Get Accelerometer and Magnetometer Data
/********************************************************************************************************************************/

void readIMU()
{
    compass.read(&accel[0], &accel[1], &accel[2], &magnetom[0], &magnetom[1], &magnetom[2]);
    gyro.read(&degGyro[0], &degGyro[1], &degGyro[2]);
}

/********************************************************************************************************************************/
//                                                      Compass Heading
/********************************************************************************************************************************/

void Compass_Heading()
{
  float mag_x;
  float mag_y;
  float cos_roll;
  float sin_roll;
  float cos_pitch;
  float sin_pitch;
  
  //saves a few processor cycles by calculating and storing values
  cos_roll = cos(roll);
  sin_roll = sin(roll);
  cos_pitch = cos(pitch);
  sin_pitch = sin(pitch);
  
  // Tilt compensated magnetic field X
  mag_x = magnetom[0] * cos_pitch + magnetom[1] * sin_roll * sin_pitch + magnetom[2] * cos_roll * sin_pitch;
  // Tilt compensated magnetic field Y
  mag_y = magnetom[1] * cos_roll - magnetom[2] * sin_roll;
  // Magnetic Heading
  yaw = atan2(-mag_x, mag_y);
  
  //make negative angles positive
  if(yaw < 0) yaw = yaw + TWO_PI;
  
  yaw = yaw * RADTODEGREE;
}

/********************************************************************************************************************************/
//                                                      getAttitude
/********************************************************************************************************************************/

void getAttitude()
{
  float temp1[3];
  float temp2[3];
  float xAxis[3] = {1.0f, 0.0f, 0.0f};
  
  // GET PITCH
  // Using y-z-plane-component/x-component of gravity vector    
  pitch = -atan2(accel[0], sqrt((accel[1] * accel[1]) + (accel[2] * accel[2])));
  
  //printf("P = %f", pitch);
  
  // GET ROLL
  // Compensate pitch of gravity vector 
  temp1[0] = (accel[1] * xAxis[2]) - (accel[2] * xAxis[1]);
  temp1[1] = (accel[2] * xAxis[0]) - (accel[0] * xAxis[2]);
  temp1[2] = (accel[0] * xAxis[1]) - (accel[1] * xAxis[0]);
  
  temp2[0] = (xAxis[1] * temp1[2]) - (xAxis[2] * temp1[1]);
  temp2[1] = (xAxis[2] * temp1[0]) - (xAxis[0] * temp1[2]);
  temp2[2] = (xAxis[0] * temp1[1]) - (xAxis[1] * temp1[0]);
  
  // Since we compensated for pitch, x-z-plane-component equals z-component:
  roll = atan2(temp2[1], temp2[2]);
}


/********************************************************************************************************************************/
//                                                      compensateAcc
/********************************************************************************************************************************/
/*
void compensateAcc() 
{
    // Compensate accelerometer error
    accel[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
    accel[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
    accel[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
}
*/

/********************************************************************************************************************************/
//                                                      rotateBTI
/********************************************************************************************************************************/

//rotation from the body frame to the inertial frame
void rotateBTI()
{
  float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw;
  
  float gravity[3] = {0, 0, -GRAVITY};
  
  float rIB[3][3];  
  
  //saves a few processor cycles by calculating and storing values
  
  //Serial.print("Roll");
  cos_roll = cos(roll);
  //Serial.print(cos_roll, 4);
  //Serial.print("    ");
  sin_roll = sin(roll);
  //Serial.print(sin_roll, 4);
  //Serial.print("    ");
  
  //Serial.print("Pitch");
  cos_pitch = cos(pitch);
  //Serial.print(cos_pitch, 4);
  //Serial.print("    ");
  sin_pitch = sin(pitch);
  //Serial.print(sin_pitch, 4);
  //Serial.print("    ");
  
  //Serial.print("YAW");
  cos_yaw = cos(yaw);
  //Serial.print(cos_yaw, 4);
  //Serial.print("    ");
  sin_yaw = sin(yaw);
  //Serial.print(sin_yaw, 4);
  //Serial.print("    ");
  
  rIB[0][0] = cos_yaw * cos_pitch;
  rIB[0][1] = (cos_yaw * sin_roll * sin_pitch) - (cos_roll * sin_yaw);
  rIB[0][2] = (sin_roll * sin_yaw) + (cos_roll * cos_yaw * sin_pitch);
  rIB[1][0] = cos_pitch * sin_yaw;
  rIB[1][1] = (cos_roll * cos_yaw) + (sin_roll * sin_yaw * sin_pitch);
  rIB[1][2] = (cos_roll * sin_yaw * sin_pitch) - (cos_yaw * sin_roll);
  rIB[2][0] = -1 * sin_pitch;
  rIB[2][1] = cos_pitch * sin_roll;
  rIB[2][2] = cos_roll * cos_pitch;
  
  for(int i = 0;  i < 3; i++)
  {
    aP[i] = inertialAccel[i];
    inertialAccel[i] = rIB[i][0] * accel[0] + rIB[i][1] * accel[1] + rIB[i][2] * accel[2];
    inertialAccel[i] = inertialAccel[i] + gravity[i];
  }
}



int updateAngles()
{
    readIMU();
    if((accel[0] <= THRESHOLD) && (accel[1] <= THRESHOLD) && (accel[2] <= THRESHOLD))
    {
        //float prevYaw = yaw;
        getAttitude();
        Compass_Heading();
        //float absOfYawDiff = sqrt((prevYaw - yaw) * (prevYaw - yaw));    
        //check for noise
        //if((absOfYawDiff >= 30) && (absOfYawDiff <= (360.0f - 30)))
        //{
            return 1;
        //}
        //return 0;
        
    }
    return 0;
    //rotateBTI();
}

float getGyro()
{
    float gx, gy, gz;
    gyro.read(&gx,&gy,&gz);
    return gz;
}