// OK
// 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 "IMUpartial.h"


#define time_display_interval 100


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

extern EulerAttitude attitude;


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

volatile char flagIT=0;

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

	
	// set fractionnal mode calculus
	CORCONbits.IF=0;
		
	InitTimer1();


	for (;;)
	{
	
	if (flagIT)
		{
		flagIT=0;
		/* OK
		// static test
		// 
		wmeasure[0]=0; 
		wmeasure[1]=0;
		wmeasure[2]=0;
		*/
		
		// OK valide 1437=99.96deg/s
		// => 0.1999deg/IT
		// pour time_display_interval 100
		// t=200ms on a
		// phitheorik=19.99deg
		// phifcteuler= 3400=(3400/32768)*PI=18.78deg
		// phicalcule avec R à la calculatrice=atan(11146/30279)=20.2deg
		// poutt=200ms
		// phitheorik=40deg
		// phifcteuler= 32deg
		// phicalcule avec R à la calculatrice=atan(11146/30279)=39.86
		// CONCLUSION: calcul de R OK mais on retrouve l'erreur du à la fct _atan du dspic pour angle>25 deg environ
		wmeasure[0]=1437; 
		wmeasure[1]=0;
		wmeasure[2]=0;
		
		/*
		// OK valide 57=4deg/s
		wmeasure[0]=57; 
		wmeasure[1]=0;
		wmeasure[2]=0;
		*/
		
		////////////
		DCMupdate(wmeasure);
		DCMtoEuler();
		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);
			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
	//=> xx *Tcy*PreDiv
	// ici Fcy=80M/2=40Mhz =>25ns , PreDiv=T1_PS_1_8 =8
	// 10000*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
}