// KO
// test calcul en point fixe



#include <p33Fxxxx.h>
#include <stdio.h>
#include <timer.h>
#include <uart.h>
#include <dsp.h>
#include  <libq.h>

#include "..\..\drivers\delay.h"
#include "..\..\drivers\drv6DOF.h"
#include "..\..\drivers\drvI2C.h"

#include "IMUpartial.h"


#define time_display_interval 256


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

void InitTimer1();
void InitPORT_AND_PIN() ;
void InitClock2();



// create global
Dof6Accelero accelero;
Dof6Gyro gyro;
extern EulerAttitude attitude;
extern IIRTransposedStruct IIRFilterStruct;

volatile unsigned long tick=0;
unsigned long next_time=time_display_interval;

volatile char flagIT=0;
fractional wx_min=-32768; // val max gyro= -2000deg/s
fractional wmeasure[3]; // gyro measure array


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


int main (void)
{	int idAcc,idGyro;
	
	InitClock2() ; //init oscillateur
	InitPORT_AND_PIN(); // init broche remapable
	InitUART1(); // 
	InitUART1(); // init UART
	Init6DOF();
	
	idAcc=GetWhoAmIAcc(); // must return 0xE5
	idGyro=GetWhoAmIGyro(); // must return 0x69
	sprintf(buffTX,"ACC Id:%d GYRO Id:%d  \n\r",idAcc,idGyro);
	UART_puts(buffTX); // hello msg
	
	// OFFSET CALIBRATION
	CalcAverageOffsetAccelerometer(&accelero); // calculate offset accelo from measure
	CalcAverageOffsetGyro(&gyro);// calculate offset gyro from measure

	// set fractionnal mode calculus
	CORCONbits.IF=0;
		
	InitTimer1();
	IIRTransposedInit ( &IIRFilterStruct );
	int resultI2C=0;
	for (;;)
	{
	//LATAbits.LATA1=!LATAbits.LATA1;	
	//resultI2C=getRawGyroData(&gyro); // 200 us data bias  compensated
	//if (resultI2C<0) UART_puts(buffKOI2C);
	//	DCMupdate(wmeasure); //9.2us
	//	normalize(); // 25.2us -9.2us =16u
	//	DCMtoEuler(); //18us
	
	if (flagIT)
		{
		flagIT=0;
		
		resultI2C=getGyroData(&gyro); // data bias  compensated
		if (resultI2C<0) UART_puts(buffKOI2C);
		IIRTransposed(1,&wmeasure[0],&gyro.data[0],&IIRFilterStruct);
		IIRTransposed(1,&wmeasure[1],&gyro.data[1],&IIRFilterStruct); 
		IIRTransposed(1,&wmeasure[2],&gyro.data[2],&IIRFilterStruct);
		//wmeasure[0]=gyro.data[0]; 
		//wmeasure[1]=gyro.data[1];
		//wmeasure[2]=gyro.data[2];
		//DCMupdate(wmeasure);
		//normalize();
		if (tick==next_time)
			{
			LATAbits.LATA1=!LATAbits.LATA1;
			DCMtoEuler();	
			next_time=tick+time_display_interval; //
			//sprintf(buffTX,"phi:%d   teta:%d      psi:%d       \n\r",attitude.phi,attitude.teta,attitude.psi);
			sprintf(buffTX,"phi:%d   teta:%d      psi:%d       \n\r",wmeasure[0],wmeasure[1],wmeasure[2]);
			UART_puts(buffTX);
			}
		} 
		
 
	}
}

void __attribute__((__interrupt__)) _T1Interrupt(void)
{
//	LATAbits.LATA1=!	LATAbits.LATA1;
	tick++;
	flagIT=1; // Pour traitement différé
    WriteTimer1(0);
    IFS0bits.T1IF = 0;    /* Clear Timer interrupt flag */
}  

void InitTimer1()
{
	/* Enable Timer1 Interrupt and Priority to "1" */
    ConfigIntTimer1(T1_INT_PRIOR_1 & T1_INT_ON);
    WriteTimer1(0);
	tick=0;
    OpenTimer1(T1_ON & T1_GATE_OFF & T1_IDLE_STOP &
               T1_PS_1_8 & T1_SYNC_EXT_OFF &
               T1_SOURCE_INT, 10000);  //TIMER1 à 2ms
	// prinicpe timer: comptage au rythme de Tcy*PreDiv
	// Si comptage=PER= 39960 alors TM1RIF ON
	//=> 4495 *Tcy*PreDiv
	// ici Fcy=80M/2=40Mhz =>25ns , PreDiv=T1_PS_1_8 =8
	// 4495*25ns*8=2ms

}

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
	
	AD1PCFGLbits.PCFG1=1; //AN1/RA1 is digital
	TRISAbits.TRISA1=0;  // RA1 out

}

	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
}