//KO

#include <p33FJ64MC802.h>

#include <stdio.h>
#include "ConfigSystem.h"

#include "../drivers/delay.h" 
#include "../drivers/drv6DOF.h" 
#include "../drivers/drvI2C.h" 
#include "../drivers/drvUART.h" 

//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 InitPORT_AND_PIN();
void InitClock2() ;

// create global
Dof6Accelero accelero;
Dof6Gyro gyro;

unsigned char buffTX[50]={'i','u','t','\n','\0'};
unsigned char buffKOI2C[5]={'K','0','\n','\0'};


int main(void)
{	int b;
	int idAcc,idGyro;
	// INIT
	InitClock2();// init clock system (PLL=> Fcy=40Mhz)
	InitPORT_AND_PIN(); // init broche remapable
	InitUART1(); // init UART
	Init6DOF();
	
	
	idAcc=GetWhoAmIAcc(); // must return 0xE5
	idGyro=GetWhoAmIGyro(); // must return 0x69
	sprintf(buffTX,"GYRO Id:%d  \n\r",idGyro);
	// NOT USE getOffsetAccelerometer(&Accelero); // get offset from accelero
	
	// OFFSET CALIBRATION
	CalcAverageOffsetAccelerometer(&accelero); // calculate offset from measure
	CalcAverageOffsetGyro(&gyro);
	
	UART_puts(buffTX); // hello msg
	int resultI2C=0;
	for(;;)
		{
		
		//getRawAccelerometerData(&accelero);	// get raw data without bias compensation
		//getAccelerometerData(&accelero); // data 0g bias compensated
		
		//getRawGyroData(&gyro); // data bias NO compensated
		resultI2C=getRawGyroData(&gyro); // data bias  compensated
		if (resultI2C<0) UART_puts(buffKOI2C);
		// RS232 display accelero
		//sprintf(buffTX,"x:%d   y:%d      z:%d       \r",accelero.data[0],accelero.data[1],accelero.data[2]);
		//UART_puts(buffTX); 
		// RS232 display gyro
		sprintf(buffTX,"x:%d   y:%d      z:%d       \n\r",gyro.data[0],gyro.data[1],gyro.data[2]);
		UART_puts(buffTX); 
		Delay(Delay_150mS_Cnt);
		b++;
		}	

}

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
}

void InitPORT_AND_PIN() 
{  // depend du schema de la carte
	// Rx : RP15
	// Tx:RP14
	_U1RXR=15;	                    //RX sur RP15/RB15
	_RP14R=3;                        //TX sur RP14/RB14
	
	TRISBbits.TRISB15=0; //RP15/RB15 en sortie

}
