// KO
// test calcul en point fixe



#include <p33FJ64MC802.h>
#include <stdio.h>
#include <timer.h>

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

#include <dsp.h>
#include  <libq.h>
#include "IMUpartial2.h"


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


//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;



// GLOBALS //////////////////////////////////////////////////////
fractional wx_min=-32768; // val max gyro= -2000deg/s
fractional wmeasure[3]; // gyro measure array
unsigned int cpt1ms=0;
char flagIT=0;
unsigned char BUFF[50]={'i','u','t','\n','\0'};
// END GLOBALS //////////////////////////////////////////////////////


int main (void)

{	InitClock2() ; //init oscillateur

	InitPORT_AND_PIN(); // init broche remapable
	InitUART1(); // init broche remapable


		// constant rotational test
		unsigned int gyrox=4096; 	// reading +250 deg/s  	phi axis - ie roll 		p=..
		unsigned int gyroy=0;		// 						teta axis - ie pitch- 	q=0 
		unsigned int gyroz=0;		// 						psi axis -ie yaw-   	r=0	

		// set fractionnal mode calculus
		CORCONbits.IF=0;
		
		wmeasure[0]=gyrox; 
		wmeasure[1]=gyroy;
		wmeasure[2]=gyroz;
	
	



	for (;;)
	{
		//DCMupdate(wmeasure); //9.2us
		
		DCMtoEuler(); //11.7 - 9.2 = 2.5us
		//LATAbits.LATA1=!	LATAbits.LATA1;
		 
 
	}
}


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




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
}

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

}
