/* (C) Jens Dalsgaard Nielsen, AAU, 2014
 * Free to use, copy modify etc
 * NO warranty whatsoever
 * If you find it usefull then I will be happy :-)
 */
#include <Wire.h>


#include <TinyGPS.h>

/* DY80 komponenenter */
#include <l3g4200.h>
#include <hmc5883l.h>
#include <adxl345.h>
#include <bmp085.h>



// dagens luftryk i 0 m i Pascal saa vi kan regne hojde udfra lufttryk
#define ZEROPRESS  101400

// hvis radio skal printe halve info hver anden gang
#define HALFRADIO

hmc5883l compass;
adxl345 adxl;

/* Variable til kompas */
MagnetometerScaled scaled;
MagnetometerRaw raw;
int MilliGauss_OnThe_XAxis; 
float heading; 
float declinationAngle;
float headingDegrees;

TinyGPSPlus gps;




float temperature, pressure, atm, altitud; /* variable for trykmaaler */

int gyrox, gyroy, gyroz;/* og for gyro */

int accx, accy,accz; /* variable for accelerometer */

unsigned long t1=0,t2=0; /* for loop */

int radioloop=0;

unsigned int samplenr=0;
char toogle = 0;

// ----------------oOo-------------------
void setup()
{
  Serial.begin(1200);  // seriel interface
  Serial3.begin(38400);  // openlog - se i CONFIG.TXT paa sd kort

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

  init_pressure(); // bmp085 sensor
  init_acc();      // adxl345 sensor
  init_compass();  // hm5883 sensor
  init_gyro();     // l3g4200
  init_GPS();      // og GPS
  gps_empty();     // og toem lige input buffer
}



void loop()
{
  /* get time and let this inner loop run every xxx msec */

  t2 = millis();
  if (500 <= (t2-t1))  // er der gaaet 1/2 sekund saa gogo
  {
    t1 = t2; // saa vi husker forrige tid_log

    do_measurements();

    print_log();

    /* Det samme for radio, men kun hver 5. gang da radio kun kører 1200 baud */

    radioloop++;
    if (radioloop > 5) {
      radioloop=0; // reset counter
      print_radio();

    }
  }

  /* GPS - traek data hjem fra GPS  - skal gores hele tiden */
  while  (Serial1.available() > 0)  {
    gps.encode(Serial1.read());
  }
}


void do_measurements()
{
  bmp085_meas(&temperature, &pressure, &altitud);
  adxl.readAccel(&accx, &accy, &accz);  /* Accelerometer - gem i accx, accy,accz*/
  l3g4200DGetGyroValues(&gyrox,&gyroy,&gyroz);  /* gyro - gem i gyrox,... This will update x, y, and z with new values */
  hmc5883mea();     /* COMPASS */
}

void print_log()
{
  Serial3.print(samplenr); 
  Serial3.print(" ");

  printDateTime_log(gps.date, gps.time);                    // 2 tal (dato kl slet)
  bmp085_print_log();                                       // 3 tal - temp(C) Pres (Pa) højde(m)
  adxl345_print_log();                                      // 3 tal acc x,y,z i mG  ( 1 G ~ 9.82 m/s2)
  l3g4200_print_log();                                      // 3 tal  gyrox,y,z
  hmc5883_print_log(raw, scaled, heading, headingDegrees);  // 8 tal n´mag x,y,z raw, x,y,z skaleret, heading (heading (degr))
  gps_print_log();                                          // 3 tal - lat lon altitude(hojde) (m)

  Serial3.println(" ");                                     // linieskift

}

void print_radio()
{
  Serial.print(samplenr); 
  Serial.print(" ");

#ifdef HALFRADIO
  /* to save bandwith we print half each time */
  if (toogle) {
    toogle=0;
    bmp085_print_radio();  
    adxl345_print_radio();
    l3g4200_print_radio();
    //      hmc5883_print_radio(raw, scaled, heading, headingDegrees);      // Output the data via the serial port.
    //      gps_print_radio();
  }
  else {
    toogle = 1;
    //      bmp085_print_radio();  
    //      adxl345_print_radio();
    //      l3g4200_print_radio();
    hmc5883_print_radio(raw, scaled, heading, headingDegrees);      // Output the data via the serial port.
    gps_print_radio();
  }
#else
  // print it all
  bmp085_print_radio();  
  adxl345_print_radio();
  l3g4200_print_radio();
  hmc5883_print_radio(raw, scaled, heading, headingDegrees);      // Output the data via the serial port.
  gps_print_radio();
#endif
  Serial.println(" ");
}

void bmp085_meas(float *temperature, float * pressure, float * altitud)
{    
  *temperature = bmp085GetTemperature(bmp085ReadUT()); //MUST be called first
  *pressure = bmp085GetPressure(bmp085ReadUP());
  *altitud = calcAltitude(*pressure,ZEROPRESS); //Uncompensated caculation - in Meters  
}

void bmp085_print_log()
{

  Serial3.print(temperature); 
  Serial3.print(" ");
  Serial3.print(pressure); 
  Serial3.print(" ");
  Serial3.print(altitud); 
  Serial3.print(" ");
}

void bmp085_print_radio()
{
  Serial.print(temperature); 
  Serial.print(" ");
  Serial.print(pressure); 
  Serial.print(" ");
  Serial.print(altitud); 
  Serial.print(" ");
}

void adxl345_print_log()
{
  Serial3.print(accx*3.9); 
  Serial3.print(" ");  // 3.9 mg /LSB (dvs pr bit saa ren skalering)
  Serial3.print(accy*3.9); 
  Serial3.print(" ");
  Serial3.print(accz*3.9); 
  Serial3.print("  ");
}

void adxl345_print_radio()
{
  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("  ");
}

void hmc5883_print_radio(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  Serial.print(raw.XAxis);
  Serial.print(" ");   
  Serial.print(raw.YAxis);
  Serial.print(" ");   
  Serial.print(raw.ZAxis);
  Serial.print(" ");  
  Serial.print(scaled.XAxis);
  Serial.print(" ");   
  Serial.print(scaled.YAxis);
  Serial.print(" ");   
  Serial.print(scaled.ZAxis);
  Serial.print(" ");

  Serial.print(heading);
  Serial.print(" ");
  Serial.print(headingDegrees);
  Serial.print(" ");
}

void hmc5883_print_log(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees)
{
  Serial3.print(raw.XAxis);
  Serial3.print(" ");   
  Serial3.print(raw.YAxis);
  Serial3.print(" ");   
  Serial3.print(raw.ZAxis);
  Serial3.print(" ");  
  Serial3.print(scaled.XAxis);
  Serial3.print(" ");   
  Serial3.print(scaled.YAxis);
  Serial3.print(" ");   
  Serial3.print(scaled.ZAxis);
  Serial3.print(" ");

  Serial3.print(heading);
  Serial3.print(" ");
  Serial3.print(headingDegrees);
  Serial3.print(" ");
}

void l3g4200_print_radio()
{
  Serial.print(gyrox); 
  Serial.print(" ");
  Serial.print(gyroy); 
  Serial.print(" ");
  Serial.print(gyroz); 
  Serial.print("  ");
}

void l3g4200_print_log()
{
  Serial3.print(gyrox); 
  Serial3.print(" ");
  Serial3.print(gyroy); 
  Serial3.print(" ");
  Serial3.print(gyroz); 
  Serial3.print("  ");
}

void gps_print_radio()
{
  Serial.print(gps.location.lat(),4); 
  Serial.print(" ");
  Serial.print(gps.location.lng(),4); 
  Serial.print(" ");
  Serial.print(gps.altitude.meters()); 
  Serial.print(" ");
}

void gps_print_log()
{
  Serial3.print(gps.location.lat(),4); 
  Serial3.print(" ");
  Serial3.print(gps.location.lng(),4); 
  Serial3.print(" ");
  Serial3.print(gps.altitude.meters()); 
  Serial3.print(" ");

}

void gps_empty()
{
  char c;
  while  (Serial1.available() > 0)  
    c= Serial1.read();
}

static void printDateTime_radio(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial.print("* ");
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial.print(sz);
  }

  if (!t.isValid())
  {
    Serial.print("* ");
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial.print(sz);
  }

}


static void printDateTime_log(TinyGPSDate &d, TinyGPSTime &t)
{
  if (!d.isValid())
  {
    Serial3.print("* ");
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d/%02d/%02d ", d.month(), d.day(), d.year());
    Serial3.print(sz);
  }

  if (!t.isValid())
  {
    Serial3.print("* ");
  }
  else
  {
    char sz[32];
    sprintf(sz, "%02d:%02d:%02d ", t.hour(), t.minute(), t.second());
    Serial3.print(sz);
  }
}

void hmc5883mea()
{
  // tror nok maalinger er milli gauss
  // 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/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);
  // Output the data down the serial port.
}

void init_gyro()
{
  l3g4200DSetup(500); // Configure L3G4200  - 250, 500 or 2000 deg/sec
}

void init_acc()
{
  adxl.powerOn();
}

void  init_compass()
{
  int error;
  compass = hmc5883l(); // Construct a new HMC5883 compass.
  error = compass.SetScale(1.3); // Set the scale of the compass.
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
}

void init_pressure()
{
  bmp085Calibration();
}

void init_GPS()
{
  Serial1.begin(9600);
}




















