/************************************************************************
*                                                                                                                                        
*    Test du module Pmod IMU 9 axes + Baromètre  (basé sur le programme de Jim Lindblom)                                        
*                                                                    
*************************************************************************
* Description:  Pmod_NAV                                              
* Toutes les données (accéléromètre, gyroscope, magnétomètre)  sont affichées 
* dans le moniteur série
*  
* Matériel
*        1. Arduino Uno                                       
*        2. Module Pmod NAV (télécharger la bibliothèque 
*        https://github.com/sparkfun/SparkFun_LSM9DS1_Arduino_Library)
*        Licence Beerware
*        
* Câblage           
*        Module<----------> Arduino
*        J1 broche 6        3.3V
*        J1 broche 5        GND
*        J1 broche 4        A5
*        J1 broche 2        A4
************************************************************************/
// Appel des bibliothèques
#include <Wire.h>
#include <SparkFunLSM9DS1.h>

// Déclaration des adresses du module
#define LSM9DS1_M  0x1E 
#define LSM9DS1_AG  0x6B 

LSM9DS1 imu;                                // création de l'objet imu

// Configuration du module
#define PRINT_CALCULATED
#define PRINT_SPEED 250 
static unsigned long lastPrint = 0; 

// Le champ magnétique terrestre varie en fonction de sa localisation.
// Il faut ajouter ou soustraire une constante pour obtenir la bonne valeur
// du champ magnétique à l'aide du site suivant
// http://www.ngdc.noaa.gov/geomag-web/#declination
#define DECLINATION -0.33 // Declinaison (en degrés) pour Paris.

void setup(void) 
{
   Serial.begin(115200);                                // initialisation de la liaison série 
   imu.settings.device.commInterface = IMU_MODE_I2C;    // initialisation du module
   imu.settings.device.mAddress = LSM9DS1_M;
   imu.settings.device.agAddress = LSM9DS1_AG;
   if (!imu.begin())
  {
    Serial.println("Probleme de communication avec le LSM9DS1.");
    while (1);
  }
}

void loop() 
{
  
  if ( imu.gyroAvailable() )
  {
    imu.readGyro();             // acquisition des données du gyroscope
  }
  if ( imu.accelAvailable() )
  {
    imu.readAccel();           // acquisition des données de l'accéléromètre
  }
  if ( imu.magAvailable() )
  {
    imu.readMag();             // acquisition du magnétomètre
  }
  
  if ((lastPrint + PRINT_SPEED) < millis())
  {
    printGyro();  // Print "G: gx, gy, gz"
    printAccel(); // Print "A: ax, ay, az"
    printMag();   // Print "M: mx, my, mz"
    
    printAttitude(imu.ax, imu.ay, imu.az,-imu.my, -imu.mx, imu.mz);
    Serial.println();
    
    lastPrint = millis(); 
  }
}

void printGyro()
{
  
  Serial.print("G: ");
#ifdef PRINT_CALCULATED
  Serial.print(imu.calcGyro(imu.gx), 2);
  Serial.print(", ");
  Serial.print(imu.calcGyro(imu.gy), 2);
  Serial.print(", ");
  Serial.print(imu.calcGyro(imu.gz), 2);
  Serial.println(" deg/s");
#elif defined PRINT_RAW
  Serial.print(imu.gx);
  Serial.print(", ");
  Serial.print(imu.gy);
  Serial.print(", ");
  Serial.println(imu.gz);
#endif
}

void printAccel()
{  
  
  Serial.print("A: ");
#ifdef PRINT_CALCULATED
  Serial.print(imu.calcAccel(imu.ax), 2);
  Serial.print(", ");
  Serial.print(imu.calcAccel(imu.ay), 2);
  Serial.print(", ");
  Serial.print(imu.calcAccel(imu.az), 2);
  Serial.println(" g");
#elif defined PRINT_RAW 
  Serial.print(imu.ax);
  Serial.print(", ");
  Serial.print(imu.ay);
  Serial.print(", ");
  Serial.println(imu.az);
#endif

}

void printMag()
{  
 
  Serial.print("M: ");
#ifdef PRINT_CALCULATED
  Serial.print(imu.calcMag(imu.mx), 2);
  Serial.print(", ");
  Serial.print(imu.calcMag(imu.my), 2);
  Serial.print(", ");
  Serial.print(imu.calcMag(imu.mz), 2);
  Serial.println(" gauss");
#elif defined PRINT_RAW
  Serial.print(imu.mx);
  Serial.print(", ");
  Serial.print(imu.my);
  Serial.print(", ");
  Serial.println(imu.mz);
#endif
}


void printAttitude(float ax, float ay, float az, float mx, float my, float mz)
{
  float roll = atan2(ay, az);
  float pitch = atan2(-ax, sqrt(ay * ay + az * az));
  
  float heading;
  if (my == 0)
    heading = (mx < 0) ? PI : 0;
  else
    heading = atan2(mx, my);
    
  heading -= DECLINATION * PI / 180;
  
  if (heading > PI) heading -= (2 * PI);
  else if (heading < -PI) heading += (2 * PI);
  else if (heading < 0) heading += 2 * PI;
  
  
  heading *= 180.0 / PI;
  pitch *= 180.0 / PI;
  roll  *= 180.0 / PI;
  
  Serial.print("Pitch, Roll: ");
  Serial.print(pitch, 2);
  Serial.print(", ");
  Serial.println(roll, 2);
  Serial.print("Heading: "); Serial.println(heading, 2);
}
