
#include <Wire.h>
#include <ADXL345.h>
#include <bmp085.h>

#define PRES_0M 101325.0


// for accelerometer
const float alpha = 0.5;

double fXg = 0;
double fYg = 0;
double fZg = 0;

ADXL345 acc;


// for pressure

float temp,atm,alt;
long pres;

// for hmc5883l compas
#include <hmc5883l.h>

hmc5883l compass; // Store our compass as a variable.
int hmc5883lError = 0; // Record any errors that may occur in the compass.

// for gyro

#include <l3g4200.h> 
int gX,gY,gZ;

void initCompass()
{
  compass = hmc5883l(); // Construct a new HMC5883 compass.

  Serial.println("Setting scale to +/- 1.3 Ga");
  hmc5883lError = compass.SetScale(1.3); // Set the scale of the compass.
  if(hmc5883lError != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(hmc5883lError));

  Serial.println("Setting measurement mode to continous.");
  hmc5883lError = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
  if(hmc5883lError != 0) // If there is an error, print it out.
    Serial.println(compass.GetErrorText(hmc5883lError));
}

void setup()
{
  acc.begin();

  bmp085_init();
  initCompass();
  Serial.begin(9600);
  l3g4200DSetup(2000); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500);
}

void doPressure()
{
  temp = bmp085Temp(); 
  pres = bmp085Pressure(); 
  Serial.print("BMP085 ");
  atm = pres / PRES_0M; // "standard atmosphere"
  alt = bmp085PascalToMeter(pres,PRES_0M);  
  Serial.print(temp);
  Serial.print(" C ");

  Serial.print(pres);
  Serial.print(" Pa ");

  Serial.print(atm);
  Serial.print(" (rel atm) ");

  Serial.print(alt);
  Serial.print( " m ");
}

void doAcc()
{
  double pitch, roll, Xg, Yg, Zg;
  acc.read(&Xg, &Yg, &Zg);

  //Low Pass Filter
  fXg = Xg * alpha + (fXg * (1.0 - alpha));
  fYg = Yg * alpha + (fYg * (1.0 - alpha));
  fZg = Zg * alpha + (fZg * (1.0 - alpha));
  Serial.print(" ADXL345 ");
  Serial.print(fXg); 
  Serial.print(" ");
  Serial.print(fYg); 
  Serial.print(" ");
  Serial.print(fZg);  
  Serial.print(" ");
}


void doCompass()
{
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();

  // Values are accessed like so:
  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  float heading = atan2(scaled.YAxis, scaled.XAxis);

  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here: http://www.magnetic-declination.com/
  // Mine is: 2� 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  float declinationAngle = 0.0457;
  heading += declinationAngle;

  // Correct for when signs are reversed.
  if(heading < 0)
    heading += 2*PI;

  // Check for wrap due to addition of declination.
  if(heading > 2*PI)
    heading -= 2*PI;

  // Convert radians to degrees for readability.
  float headingDegrees = heading * 180/M_PI; 

  // Normally we would delay the application by 66ms to allow the loop
  // to run at 15Hz (default bandwidth for the hmc5883l).
  // However since we have a long serial out (104ms at 9600) we will let
  // it run at its natural speed.
  // delay(66);

  Serial.print(" HMC5883L");
#include <l3g4200.h> #include <l3g4200.h> 
  Serial.print("Raw:\t");
  Serial.print(raw.XAxis);
  Serial.print("   ");   
  Serial.print(raw.YAxis);
  Serial.print("   ");   
  Serial.print(raw.ZAxis);
  Serial.print("   \tScaled:\t");

  Serial.print(scaled.XAxis);
  Serial.print("   ");   
  Serial.print(scaled.YAxis);
  Serial.print("   ");   
  Serial.print(scaled.ZAxis);

  Serial.print("   \tHeading:\t");
  Serial.print(heading);
  Serial.print(" Radians   \t");
  Serial.print(headingDegrees);
  Serial.print(" Degrees   \t");
}

void doGyro()
{
   l3g4200DGetGyroValues(&gX,&gY,&gZ);
   Serial.print(" GYRO");
  Serial.print("X:");
  Serial.print(gX);

  Serial.print(" Y:");
  Serial.print(gY);

  Serial.print(" Z:");
  Serial.print(gZ);
}

void loop()
{
  doPressure();
  doAcc();
  doCompass();
  doGyro();
  Serial.println(" ");
    delay(10);
}







 
