/************************************************************************
*                                                                                                                                        
*    Test du module Pmod boussole                                      
*                                                                    
*************************************************************************
* Description:  Pmod_GYRO                                              
* Les 3 composantes (X, Y, Z) du champ magnétique puis son module et son angle 
* sont affichés dans le moniteur série
*  
* Matériel
*        1. Arduino Uno                                       
*        2. Module Pmod CMPS 
*            
************************************************************************/

#include <Wire.h>                                       // appel de la bibliothèque
#define HMC5803L_Addresse 0x1E                          // adresse I2C du module

double composante_X;
double composante_Y;
double composante_Z;
double module;
double angle;

void setup()
{
  Serial.begin(9600);
  Wire.begin();                                       // initialisation du bus I2C
  Init_HMC5803L();                                    // initialisation du module boussole
}

void loop()
{
  Wire.beginTransmission(HMC5803L_Addresse);         // lancement de la mesure
  Wire.write(0x02);
  Wire.write(0x01);
  Wire.endTransmission();
  delay(10);
  Wire.requestFrom(HMC5803L_Addresse, 6);            // récupération des composantes
  if(Wire.available() <=6)
  {    
    composante_X = Wire.read() << 8 | Wire.read();
    composante_Y = Wire.read() << 8 | Wire.read();
    composante_Z = Wire.read() << 8 | Wire.read();
  }
  Serial.print("X=");
  Serial.print (composante_X);
  Serial.print('\t');
  Serial.print("Y=");
  Serial.print (composante_Y);
  Serial.print('\t');
  Serial.print("Z=");
  Serial.print (composante_Z);
  Serial.print('\t');

  // Calcul du module
  module = sqrt(composante_X * composante_X + composante_Y * composante_Y + composante_Z * composante_Z);
  Serial.print("M=");
  Serial.print(module);
  Serial.print('\t');
 
  //Calcul de l'angle
  angle= atan2(composante_Y,composante_X) * (180 / 3.14159265);        // angle en degrés
  if (angle<0) {angle=angle+360;}
  Serial.print("Angle=");  
  Serial.println(angle);

 delay(1000);
}
 
// Initialisation du module HMC5803L
void Init_HMC5803L(void)
{
  Wire.beginTransmission(HMC5803L_Addresse);
  Wire.write(0x00);
  Wire.write(0x70);      
  Wire.write(0x01);
  Wire.write(0xA0);  
  Wire.endTransmission();
}
