File content as of revision 4:a397b44a0fe8:

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

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

#define M_PI            3.1415926535897932384626433832795f
#define TWO_PI          6.283185307179586476925286766559f
#define RADTODEGREE     57.295779513082320876798154814105f
#define GRAVITY       1.0f
#define THRESHOLD 1.1f

// "accel x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
#define ACCEL_X_MIN ((float) -276)
#define ACCEL_X_MAX ((float) 236)
#define ACCEL_Y_MIN ((float) -261)
#define ACCEL_Y_MAX ((float) 245)
#define ACCEL_Z_MIN ((float) -284)
#define ACCEL_Z_MAX ((float) 220)

// "magn x,y,z (min/max) = X_MIN/X_MAX  Y_MIN/Y_MAX  Z_MIN/Z_MAX"
#define MAGN_X_MIN (-0.370909f)
#define MAGN_X_MAX (0.569091f)
#define MAGN_Y_MIN (-0.516364f)
#define MAGN_Y_MAX (0.392727f)
#define MAGN_Z_MIN (-0.618367f)
#define MAGN_Z_MAX (0.408163f)

// Sensor calibration scale and offset values
#define ACCEL_X_OFFSET ((ACCEL_X_MIN + ACCEL_X_MAX) / 2.0f)
#define ACCEL_Y_OFFSET ((ACCEL_Y_MIN + ACCEL_Y_MAX) / 2.0f)
#define ACCEL_Z_OFFSET ((ACCEL_Z_MIN + ACCEL_Z_MAX) / 2.0f)
#define ACCEL_X_SCALE (GRAVITY / (ACCEL_X_MAX - ACCEL_X_OFFSET))
#define ACCEL_Y_SCALE (GRAVITY / (ACCEL_Y_MAX - ACCEL_Y_OFFSET))
#define ACCEL_Z_SCALE (GRAVITY / (ACCEL_Z_MAX - ACCEL_Z_OFFSET))

#define MAGN_X_OFFSET ((MAGN_X_MIN + MAGN_X_MAX) / 2.0f)
#define MAGN_Y_OFFSET ((MAGN_Y_MIN + MAGN_Y_MAX) / 2.0f)
#define MAGN_Z_OFFSET ((MAGN_Z_MIN + MAGN_Z_MAX) / 2.0f)
#define MAGN_X_SCALE (100.0f / (MAGN_X_MAX - MAGN_X_OFFSET))
#define MAGN_Y_SCALE (100.0f / (MAGN_Y_MAX - MAGN_Y_OFFSET))
#define MAGN_Z_SCALE (100.0f / (MAGN_Z_MAX - MAGN_Z_OFFSET))



//calibrated reading = (reading - offset) * scale

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]);
    //Compensate magnetometer error
    //magnetom[0] -= MAGN_X_OFFSET;
    //magnetom[1] -= MAGN_Y_OFFSET;
    //magnetom[2] -= MAGN_Z_OFFSET;
    
    //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;
    
    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]);
}

/********************************************************************************************************************************/
//                                                      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((sqrt(accel[0] * accel[0]) <= THRESHOLD) && (sqrt(accel[1] * accel[1]) <= THRESHOLD) && (sqrt(accel[2] * 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;
}