//KO

#include <p33FJ64MC802.h>

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

#include "..../../../drivers/delay.h" 
#include "../Driver6DOF/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
	InitClockNoSwitch();// 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++;
		}	

}
