
// test calcul en point fixe



#include <p33Fxxxx.h>
#include <stdio.h>


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

// Types
struct ww { int W0 ; int W1 ; } ;
union longww { long  WW ; struct ww _ ; } ;




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

}

	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 wx,wy,wz; // gyro measure
// coeeficient permettant d'obtenir angle pour un deltaT=2ms => deltaPhi=4 deg=0.06991 
								//= 1144 en Q1.14 et =2288 en Q15
fractional deltaPhi_x,deltaPhi_x2;
fractional deltaPhiVector[]={0,0,0};
fractional deltaPhiVectorTemp[]={0,0,0};


fractional	OmegaRot[]={	0x4000	,0		,	0	,
							0		,0x4000	, 	0	,
							0		,0		,0x4000	
						}; // 1 coded in Q1.14 (0b  0 1.xxxxxxxxxxxxxx)=  0.5 coded in Q15

					
fractional	R[]=	{	0x4000	,0		,		0	,
							0		,0x4000	, 	0	,
							0		,0		,0x4000	
					}; // 1 coded in Q1.14 (0b  0 1.xxxxxxxxxxxxxx) 	// 0.5 coded in Q15

					
fractional	Rtemp[]=	{		0		,0		,	0	,
							0		,0		, 	0	,
							0		,0		,	0		
					};
float fDeltaPhi_x1,fDeltaPhi_x2;




int main (void)

{	InitClock2() ; //init oscillateur

	InitPORT_AND_PIN(); // init broche remapable


		fractional w[]={0,0,0};
		fractional wresult[]={0,0,0};
		fractional k_OmegaToAngle=2288 ;
			
		// test mutplication en Q15
		 wx_min=-32768;
		 k_OmegaToAngle=2288 ;
	
		CORCONbits.IF=0;
		// SOL1: KO
		//deltaPhi_x=(wx_min*k_OmegaToAngle); // resultat en Q15
		// KO ne marche pas recupere les 16 bits de poids faible 
		
		// SOL2: OK
		union longww crossaccum ;
		crossaccum.WW = __builtin_mulss( wx_min, k_OmegaToAngle ) ;
		deltaPhi_x = crossaccum._.W1 ; // recupere les 16 bits de point fort de la multiplication
		deltaPhi_x2=crossaccum.WW>>15;
		
		// SOL3: OK
		signed long int test;
		signed int test2;
		test=__builtin_mulss( wx_min, k_OmegaToAngle );
		test2=test>>15;
		
		//Solution reference : utilise fonction dsp de la librairie
		// opermet de comparer les diffentes SOL
		VectorScale(3,wresult,w,k_OmegaToAngle);
		
		// BILAN: SOL2 et SOL3 OK sauf pour -1.0*-1.0
		// cas -32768*-32678 : sol2 et 3 resultat=0x8000 : faux
		// sol reference res=7FFF OK 
		// analyse diffrence: C:\Program Files\Microchip\MPLAB C30\src\dsp\asm\vscl.s
		// diffrence car VectorScale utilise l'accumulateur (mpy	w4*w5,a	) avec resultat sur 32+1 bits extra 
		//pour obtenir resultat juste + extension de bit jusqu'à bit 40
		// resultat fibal obtenu avec decalage ( sac.r	a,[w1++])
		/////////////////////////////////////////////////////////////////////////////////////
		
		//Test Influence du CORCONbits sur mulss
		// sans influence en signé 
		CORCONbits.IF=0;
		test=__builtin_mulss( 182, 182 );
		test2=test>>15;
		k_OmegaToAngle=0x00B6;
		w[0]=182;
		VectorScale(3,wresult,w,k_OmegaToAngle);
		
		CORCONbits.IF=1;
		test=__builtin_mulss( 182, 182 );
		test2=test>>15;
		w[0]=182;
		k_OmegaToAngle=182;
		VectorScale(3,wresult,w,k_OmegaToAngle);
		
		// test depassement
		CORCONbits.IF=0;
		// test addition avec  
		test=	__builtin_mulss ( (0x7FFF >>1), (0x7FFF >>1) );
		test+=	__builtin_mulss ( (0x100>>1), (0x100>>1 ));
	
		test=test>>14;
		
		fDeltaPhi_x1= Fract2Float  (deltaPhi_x); 
		fDeltaPhi_x2= Fract2Float  ((deltaPhi_x>>1)); // convert Q15 to Q1.14
	
			// 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	

		
		
		// test trigo 16-Bit Language Tools Libraries.pdf
		////////////////////////////////////////////////////////////////////////////////////////
		// 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
			gyrox=4096; // just for breakpoint
		angle=_Q15asinByPI(16384); //asin(0.5)=30deg result 5461=29.99deg
			gyrox=4096;  // just for breakpoint
		angle=_Q15asinByPI(-8192);//sin(-0.25)=-14,4deg result -2635=0.08041*PI=14.14deg
			gyrox=4096; // just for breakpoint
		angle=_Q15asinByPI(32768);	
			gyrox=4096; // just for breakpoint
		////////////////////////////////////////////////////////////////////////////////////////

		// atan test function OK
		x=Q15(0.466); //0.466 = tan(25deg) rq xmax=0,9999 corespond a angle max =45 = 25735 (voir p216 pdf Tool librairie)
		angle=_Q15atan(x); // result is 37D0 => 14288*45/25735 =25 deg
		angle=_Q15atanYByX(Q15(0.9063),Q15(0.4226)) ; // x=sin(25deg) y=cos(25deg);
		
		////////////////////////////////////////////////////////////////////////////////////////
		// sqrt test function
		fractional norm;
		norm=_Q15sqrt(0);
		norm=_Q15sqrt(0x7FFF);
		norm=_Q15sqrt(0x4000);
		norm=_Q15sqrt(0x2000);
		norm=_Q15sqrt(Q15(0.68)); // result 0.82 = x698d en q15
		////////////////////////////////////////////////////////////////////////////////////////
		
		
		fractional coef1[3];fractional vect2[3];	
		
		
	
	for (;;)
	{
		 
 	//Q1.14 ie : 1 sign bit + 1 bit integer part +14 fractional part
	//   -2.0<  Q1.14 value< (2.0- 1/2^14)	
	// LSB= 2^14   
 	// skew matrix construct Q1.14
	// objectif: (Imatrix+AngleSkewmatrix)
		fractional k_OmegaToAngle=1144 ; 
	// step one: AngleSkewmatrix
		deltaPhiVector[0]=gyrox; 
		deltaPhiVector[1]=gyroy;
		deltaPhiVector[2]=gyroz;

		VectorScale(3,deltaPhiVector,deltaPhiVector,k_OmegaToAngle); // convert w to angle
					//	VectorAdd(3,deltaPhiVector,deltaPhiVector,deltaPhiVector); // scale result to Q1.14 (2 multiply)
		
		// skew construct
		// first row
		OmegaRot[1]=-deltaPhiVector[2]; //-phi_z
		OmegaRot[2]=+deltaPhiVector[1]; //+phi_y
		// second row
		OmegaRot[3]=deltaPhiVector[2]; //+phi_z
		OmegaRot[5]=-deltaPhiVector[0]; //-phi_x
		// third row
		OmegaRot[6]=-deltaPhiVector[1]; //-phi_y
		OmegaRot[7]=+deltaPhiVector[0]; //phi_x
	
	// step two: Rnew=R*(I+Angleskwew)
		MatrixMultiply(3,3,3,Rtemp,R,OmegaRot); // result is scale in Q15 
		MatrixAdd(3,3,R,Rtemp,Rtemp); // so need to convert in Q1.15 : x2scaling
		
		_Q15 phi;
		_Q15 y=R[7]; 
		_Q15 x=R[8];
		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
	}
}


