
#include <l3g4200.h>
#include <hmc5883l.h>
#include <adxl345.h>
#include <bmp085.h>

#include <looptime.h>

#include <Wire.h>

/* (C) Jens Dalsgaard Nielsen, AAU, 2014
 * Free to use, copy modify etc
 * NO warranty whatsoever
 */


/* compass hmc5883l */
hmc5883l compass;

MagnetometerScaled scaled;
MagnetometerRaw raw;
int MilliGauss_OnThe_XAxis; 
float heading; 
float declinationAngle;
float headingDegrees;

/* trykmaaler bmp085*/
float temperature;
float pressure, atm, altitud;

/* gyro l3g4200 */
int gyrox, gyroy, gyroz;

/* accelerometer adxl345*/
adxl345 adxl;
int accx, accy,accz;


int init_gyro()
{
  l3g4200DSetup(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec
  delay(1500); //wait for the sensor to be ready 
  return (0); // ok
}

int init_acc()
{
  adxl.powerOn();
  return (0); // ok
}

int init_compass()
{
  int err1,err2;
  compass = hmc5883l(); // Construct a new HMC5883 compass.

  err1 = compass.SetScale(1.3); // Set the scale of the compass. return 0 if ok
 
  err2 = compass.SetMeasurementMode(Measurement_Continuous); // returns 0 if ok
  
  return (err1 + 8*err2); // 0 if alles ok
}

int init_pressure()
{
  bmp085Calibration();
  return (0); // ok
}

int e; // total err
void setup()
{
 
  Serial.begin(9600);

  Wire.begin();  // start i2c bus - forbindelse til dy80 board

  e =  init_pressure();
  e += init_acc();  
  e += init_compass();
  e += init_gyro();
}


// Output the data down the serial port.
void magOutput(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  Serial.print("  Raw:");
  Serial.print(raw.XAxis);
  Serial.print("   ");   
  Serial.print(raw.YAxis);
  Serial.print("   ");   
  Serial.print(raw.ZAxis);
  Serial.print("  Scaled:\t");

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

  Serial.print(" Heading: ");
  Serial.print(heading);
  Serial.print(" Radians ");
  Serial.print(headingDegrees);
  Serial.print(" Degr");
}


void hmc5883meaorg()
{
  // from hmc5883 code (not by JDN :-)
  // Retrive the raw values from the compass (not scaled).
  raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  scaled = compass.ReadScaledAxis();

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

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  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.
  declinationAngle = 0.0; // JDN 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.
  headingDegrees = heading * 180.0/PI; 

  // Output the data via the serial port.
  magOutput(raw, scaled, heading, headingDegrees);

  // 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);
  // Output the data down the serial port.
}

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

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

  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  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.
  declinationAngle = 0.0; // Jens 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.
  headingDegrees = heading * 180/PI; 

  // Output the data via the serial port.
//  magOutput(raw, scaled, heading, headingDegrees);

  // 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);
  // Output the data down the serial port.
}


unsigned long t1=0,t2=0;

void loop()
{
  //  goto xxx;

  t2 = millis();
  if (1000 <= (t2-t1))
  {
    t1 = t2;

    /* MAAL TRYK OG TEMP */
    temperature = bmp085GetTemperature(bmp085ReadUT()); //MUST be called first
    pressure = bmp085GetPressure(bmp085ReadUP());
    atm = pressure / 101325; // "standard atmosphere"
    altitud = calcAltitude(pressure,101400); //Uncompensated caculation - in Meters 

    Serial.print(temperature); 
    Serial.print(" C ");
    Serial.print(pressure); 
    Serial.print(" Pas ");
    Serial.print(altitud); 
    Serial.print(" m  ");

    /* Accelerometer */
    adxl.readAccel(&accx, &accy, &accz);

    Serial.print(accx*3.9); 
    Serial.print(" ");  // 3.9 mg /LSB (dvs pr bit saa ren skalering)
    Serial.print(accy*3.9); 
    Serial.print(" ");
    Serial.print(accz*3.9); 
    Serial.print(" mG  ");

    /* gyro */
    l3g4200DGetGyroValues(&gyrox,&gyroy,&gyroz);  // This will update x, y, and z with new values

    Serial.print(gyrox); 
    Serial.print(" ");
    Serial.print(gyroy); 
    Serial.print(" ");
    Serial.print(gyroz); 
    Serial.print(" degr/sec");

    /* COMPASS*/
    hmc5883mea();
    magOutput( raw, scaled, heading, headingDegrees);

    Serial.println(" !");
  }
}






