/* (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 :-)
 */

// we need a MEGA(1280/2560) for three serial ports so lets test
/*
#if defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
#else
// error
#error Wrong Architecture 1280/2560(Mega) needed
#endif

#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
// As the Arduino has only 
// WE are running 19200 baud to radio
// TX buffer is 64 byte so in rough numbers we have to wait to send 59 bytes
// which add 50 * 0.5msec = 25 msec to tx time
// SO NO HALF PACKAGES :-)

//#define HALFRADIO

// If defined then all raw and scaled are printed
// if not then i´only raw - saves bandwidth

//#define FULLPRINT

hmc5883l compass;


/* Variable til kompas */
MagnetometerScaled scaled;
MagnetometerRaw raw;

int MilliGauss_OnThe_XAxis; 
float heading, declinationAngle, headingDegrees;

TinyGPSPlus gps;

adxl345 adxl;


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(19200);  // seriel interface to radio
  Serial3.begin(38400);  // openlog - se i CONFIG.TXT paa sd kort
  // Serial1 is initialised in init_GPS

  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
  empty_gps();     // og toem lige input buffer
}



void loop()
{


  t2 = millis();          // get time and let this inner loop run every xxx msec 
  if (500 <= (t2-t1)){    // er der gaaet 1/2 sekund saa gogo
    t1 = t2;              // saa vi husker forrige tid_log

    samplenr++;
    do_measurements();    // see below

    print_log();

    // Det samme for radio, men kun hver 5. gang da radio kun kører 2400 baud in air og effektiv 1000 baud for data 

    radioloop++;
    if (radioloop > 5) { // every 6th loop == hvert 3. sekund
      radioloop=0; // reset counter
      print_radio();

    }
  }
  read_gps(); // shall be done all time because 
}


void do_measurements()
{
  bmp085_meas(&temperature, &pressure, &altitud);  // pressure and temp
  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();  // 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();      // 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();      // 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();      // 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  
}


/* JENS
 * I en del af print rutinerne er der kommenteret kode ud - så det ikk er aktiv
 * Det er for at spare på dyr båndbredde i luften
 * Se flag FULLPRINT
 */

void bmp085_print_log()
{

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

#ifdef FULLPRINT
  Serial3.print(altitud); 
  Serial3.print(" ");
#endif
}

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

#ifdef FULLPRINT
  Serial.print(altitud); 
  Serial.print(" ");
#endif
}

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( )
{
  Serial.print(raw.XAxis);
  Serial.print(" ");   
  Serial.print(raw.YAxis);
  Serial.print(" ");   
  Serial.print(raw.ZAxis);
  Serial.print(" ");  

#ifdef FULLPRINT
  Serial.print(scaled.XAxis);
  Serial.print(" ");   
  Serial.print(scaled.YAxis);
  Serial.print(" ");   
  Serial.print(scaled.ZAxis);
  Serial.print(" ");

  Serial.print(heading);
  Serial.print(" ");
#endif

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

void hmc5883_print_log( )
{
  Serial3.print(raw.XAxis);
  Serial3.print(" ");   
  Serial3.print(raw.YAxis);
  Serial3.print(" ");   
  Serial3.print(raw.ZAxis);
  Serial3.print(" ");  

#ifdef FULLPRINT
  Serial3.print(scaled.XAxis);
  Serial3.print(" ");   
  Serial3.print(scaled.YAxis);
  Serial3.print(" ");   
  Serial3.print(scaled.ZAxis);
  Serial3.print(" ");

  Serial3.print(heading);
  Serial3.print(" ");
#endif

  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 empty_gps()
{
  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/

  //JDN  declinationAngle = 0.0; // Jens Aalborg 50 micro tesla afvigelse 2grad 6' east ~ 2.1degr east ¨= ingenting
  //  heading += declinationAngle;

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

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

  // Convert radians to degrees for readability.
  headingDegrees = 360.0 - heading * 180/M_PI; // JDN added 360 -  bq rrad and degress go each way around
}

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 read_gps()
{
  while  (Serial1.available() > 0)  // GPS - traek data hjem fra GPS  - skal gores hele tiden 
    gps.encode(Serial1.read());
}

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

























