// KO
// test calcul en point fixe



#include <p33Fxxxx.h>
#include <stdio.h>
#include <timer.h>
#include <uart.h>
#include "delay.h"
#include "drvUART.h"
#include "dsp.h"
#include  <libq.h>
#include "IMU.h"



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



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


unsigned char BUFF[50]={'i','u','t','\n','\0'};

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;
	
		// testing _Q15cosPI function OK
		_Q15 angle; 	_Q15 x;
		angle=32767; // 37767=PI  16384=PI/2 0=0
		x=_Q15cosPI(angle); // result -32768 =-1.0
		angle=16363;
		x=_Q15cosPI(angle); // result=65 =0.002
		angle=24546;
		x=_Q15cosPI(angle);
		angle=0;
		x=_Q15cosPI(angle); // result=32767
		angle=0;
		
		// testing _Q15asinByPI function OK
		// input -1;+1 result in deg is (result/32768)*180deg
		angle=_Q15asinByPI(0); //result =0
			wmeasure[0]=gyrox; 
		angle=_Q15asinByPI(16384); //asin(0.5)=30deg result 5461=29.99deg
			wmeasure[0]=gyrox; 
		angle=_Q15asinByPI(-8192);//sin(-0.25)=-14,4deg result -2635=0.08041*PI=14.14deg
			wmeasure[0]=gyrox; 
		angle=_Q15asinByPI(32768);	
			wmeasure[0]=gyrox; 
		
		// testing DCMtoEuler
		// cos45=23170 in Q15 sin45=23170 result =8192 =PI/4 OK
		// cos25=29698 sin25=13848 result=4170 = 22.9deg
		//cos10=32270 sin10=5690 result=1792 = 9.8 deg
		// cos 40=25102 sin40=21063 result=5958=32.7 deg !!
		// cos-15=31651 sin-15=-8458 result=-14.5deg
		// cos-60=16384 sin-60=-28378 result=-63deg
		// quadrant 2 et 3 not tested
		//Conclusion  _Q15atanYByX dspic function accurate only if angle small!!
		// in hoovering condition OK (15deg error=3.53% : 25deg error 8%)
	
		R[8]=31651  ;// cos phi
		R[7]=-8458	 ; //sin phi
		R[6]=  29743;  // sin (teta)
		R[0]=16384	;//	cospsi
		R[3]=-28378	;//sin psi
		////////////////////////////
		
		//
		fractional norm;
		norm=_Q15sqrt(0);
		norm=_Q15sqrt(0x7FFF);
		norm=_Q15sqrt(0x4000);
		norm=_Q15sqrt(0x2000);	
		
		//testing normalize function
		R[0]=0x4000 ;
		R[1]=0	 ; 
		R[2]=  0;  
		R[3]=0	;
		R[4]=0x4000	;
		R[5]=0	;
		R[6]=0	;
		R[7]=0	;
		R[8]=0x4000;	
		normalize();
	//	MatrixMultiply(3,3,3,Rtemp,R,R); 
	//	wmeasure[0]=gyrox; 	
	for (;;)
	{
		//DCMupdate(wmeasure); //9.2us
		
		DCMtoEuler(); //11.7 - 9.2 = 2.5us
		LATAbits.LATA1=!	LATAbits.LATA1;
		 
 
	}
}

	/*
		_Q15 phi;
	
		phi=_Q15atanYByX(x,y) ; // no scaling required since tg=y/x phi is read as Q1.14
	
	//	MatrixAdd(3,3,R,Rtemp,Rtemp);
 	   gyroy=0;// just for breakpoint
 	   */

