// OK
// test simple  des fonctions I2C
// ReadByteI2C1 et ReadMultipleI2C1
// verifie si WHO_AM_I correct pour gyro et acc

#include <p33FJ64MC802.h>
#include <math.h>
#include <stdio.h>
#include "drv6DOF.h" 
#include "drvI2C.h" // test

//config oscillateur
_FOSCSEL(FNOSC_PRIPLL);								// Choix du oscillateur + PLL
_FOSC(FCKSM_CSDCMD & OSCIOFNC_OFF  & POSCMD_XT);	// Pas de basculement d'horloge
_FWDT(FWDTEN_OFF); // watchdog off;

void InitClock2() 
{   // CHOIX DES COEF DE LA PLL : voir notice OSCILLATOT
	// FOSC = FIN •(M/(N1*N2))  et Fcy=Fosc/2 = fréquence de fct des périphriques
	// ex: quartz 8Mhz et Fosc=80Mhz => Fosc=8M*(40/4)
	PLLFBD = 38;				// M = 40       
	CLKDIVbits.PLLPOST = 0;		// N1 = 2
	CLKDIVbits.PLLPRE = 0;		// N2 = 2
	OSCTUN = 0;
	RCONbits.SWDTEN = 0;

	while(OSCCONbits.LOCK != 1) {}; // attente verrouillage PLL
}


int main(void)
{	unsigned char val=0;
	unsigned char val2=0;
	unsigned char val3=0;
	unsigned char val4=0;
	unsigned char b=0;
	
	InitClock2() ;
	Init6DOF();
	val=GetWhoAmIGyro();// must return 0x69
	val2=ReadByteI2C1(ADDR_ITG3200,ITG3200_WHO_AM_I );
	ReadMultipleI2C1(ADDR_ITG3200,ITG3200_WHO_AM_I,1,&val3);
	val4=GetWhoAmIAcc(); // must return 0xE5
	for(;;)
	{
	b++;
	}	

}
