
//// version COMPLETE
#include <p33FJ64MC802.h>
#include "../drivers/delay.h"
#include "../drivers/drvI2C.h"
#include "../drivers/drv6DOF.h"
#include "../drivers/drvHMC5883L.h"
#include <math.h>
#include <stdio.h>

HMC5883L magnetometre;
HMC5883L offsetHMC5383;

void InitHMC5883L(void)
{
        WriteByteI2C1( ADDR_HMC5883L,0x00,0x70); // Configuration registre A (15hz/Normal measurement)
        Delay(Delay_75mS_Cnt);
        WriteByteI2C1( ADDR_HMC5883L,0x01,0xA0); // Configuration registre B
        Delay(Delay_75mS_Cnt);
        WriteByteI2C1( ADDR_HMC5883L,0x02,0x00); // Configuration mode register (Continuous-measurement mode)
        Delay(Delay_75mS_Cnt);
}




unsigned char GetWhoAmIHMC5883L(void) // renvoi la valeur du whoiam
{
        unsigned char varlocal;
        varlocal=ReadByteI2C1(ADDR_HMC5883L,10);
        return (varlocal);
}
 




char getRawHMC5883LData(HMC5883L * ptrHMC5883L)//renvoi la valeur en X,Y,Z dans les pointeurs data
    {
        unsigned char buff[6];
        char result;

        result=ReadMultipleI2C1(ADDR_HMC5883L,0x03, 6, buff);
        ptrHMC5883L->data[0] = ( (((int)buff[0]) << 8) | buff[1]) ;
        ptrHMC5883L->data[1]  = ((((int)buff[2]) << 8) | buff[3]);
        ptrHMC5883L->data[2]  = ((((int)buff[4]) << 8) | buff[5])  ;
        return(result);

    }

char SetValueMagOffsetXY( int x, int y)
   {
     offsetHMC5383.data[0]=x;
     offsetHMC5383.data[1]=0;
     offsetHMC5383.data[2]=y;
    }


char getCompensatedHMC5883LData(HMC5883L * ptrHMC5883L)//renvoi la valeur en X,Y,Z dans les pointeurs data
    {
        unsigned char buff[6];
        char result;

        result=ReadMultipleI2C1(ADDR_HMC5883L,0x03, 6, buff);
        ptrHMC5883L->data[0] = ( (((int)buff[0]) << 8) | buff[1]) - offsetHMC5383.data[0];
        ptrHMC5883L->data[1]  = ((((int)buff[2]) << 8) | buff[3]) - offsetHMC5383.data[1];
        ptrHMC5883L->data[2]  = ((((int)buff[4]) << 8) | buff[5]) - offsetHMC5383.data[2];
       
        return(result);
    }





void calibrateHMC5883L()
{

        int i;
        int I2C_OK;
        int Xmax;
        int Xmin;
        int Ymax;
        int Ymin;
        int OffsetY;
        int OffsetX;
        unsigned char buffTX[50]={'i','u','t','\n','\0'};
        //unsigned char buffKOI2C[5]={'K','0','\n','\0'};
        offsetHMC5383.data[1]=0 ;// z offset is set to global variable


        Xmax=-16383;
        Xmin=16383;
        Ymax=-16383;
        Ymin=16383;
      //temporisation
        int compteur=0;
        int compteurPlus=0;
        for (compteur=0;compteur<5;compteur++)
        {
           sprintf(buffTX,"%d secondes \r\n",compteur);
           UART_puts(buffTX); // envoi sur RS232 du resultat
           Delay(Delay_1S_Cnt);
        }//fin for

        sprintf(buffTX,"Start calibration: 40 secondes /faire 2 tours\r\n");
        UART_puts(buffTX); // envoi sur RS232 du resultat


	for (compteur=0;compteur<400;compteur++)
        {
            I2C_OK=getRawHMC5883LData(&magnetometre);


            if(magnetometre.data[2]>Ymax)
                {Ymax=magnetometre.data[2];}

            if(magnetometre.data[0]>Xmax)
                {Xmax=magnetometre.data[0];}

            if(magnetometre.data[2]<Ymin)
                {Ymin=magnetometre.data[2];}

            if(magnetometre.data[0]<Xmin)
                {Xmin=magnetometre.data[0];}

            if(compteur%10==0)//affiche toutes les secondes
                {compteurPlus=compteur/10;
                    sprintf(buffTX,"%d secondes sur 40 \r\n",compteurPlus);
                    UART_puts(buffTX); // envoi sur RS232 du resultat
                }

            Delay(Delay_50mS_Cnt);//permet de faire durer 40sec la boucle for

          }// fin for

        sprintf(buffTX,"Fin calibration");
        UART_puts(buffTX); // envoi sur RS232 du resultat

        offsetHMC5383.data[0]=(Xmax+Xmin)/2; //calcul de l'offset
        offsetHMC5383.data[2]=(Ymax+Ymin)/2; //calcul de l'offset

        sprintf(buffTX,"\r Offset en X: %d \r Offset en Y : %d\n",offsetHMC5383.data[0],offsetHMC5383.data[2]);
        UART_puts(buffTX); // envoi sur RS232 du resultat
             
}//fin void calibrateHMC5883L()


int CalculCap(int x,int y)//trouver l'angle
 {
   // int I2C_OK;
   // unsigned char buffTX[50]={'i','u','t','\n','\0'};
    float a,b;
    int angle;


    if (x!=0)
    {
        a=(y*1.0)/x;
        if(x>0)
        {
            b = atanf (a);
            //b=(b*360)/3.1415;
        }//if(x>0)
        else
        {if(y<0)
            {x=-x;
            y=-y;
            a=y/x;
            b = atanf (a);
            //b=(b*360)/3.1415;
            }//if(y<0)
        else
            {x=-x;
            y=-y;
            a=y/x;
            b = atanf (a);
            //b=(b*360)/3.1415;
            }//fin if(y>0)
        }//fin else
    }
    else
    {if(y>0){b=3.14159/2.0;}

     else{b=-3.14159/2.0;}
        
      }
       
    //}//for(;;)
    angle=(int)(360*b/3.1416);
    return (angle);
 }//void Arctangente()