// IMU PARTIAL

#include "IMUpartial3.h"
#include <p33Fxxxx.h>
#include <dsp.h>
#include  <libq.h>

#define Q14_OnePlus 0x4000  //01.xxxxxxx	+1  in Q2.14
#define Q14_OneMinus 0xC000  //11.xxxxxxx   -2+1=-1 in Q2.14

fractional	OmegaRot[]={	0x7FFF	,0		,	0	,
							0		,0x7FFF	, 	0	,
							0		,0		,0x7FFF	
						}; // 0 coded in Q1.15

					
fractional	R[]=	{	0x7FFF	,0		,		0	,
							0		,0x7FFF	, 	0	,
							0		,0		,0x7FFF	
					}; // 1 coded in Q1.15

					
fractional	Rtemp[]=	{	0		,0		,	0	,
							0		,0		, 	0	,
							0		,0		,	0		
					};




// coeeficient permettant d'obtenir angle pour un deltaT=2ms => deltaPhimax=4 deg=0.06991=2288 en Q15 
// k*wmax=deltaPhiMax avec wmax=2000deg/s et 1deg/s=14.375LSB=> wmax=28750
// k=2288/28750=0.07957=2607 en Q15
fractional k_OmegaToAngle=2607 ;
								
								
fractional deltaPhiVector[]={0,0,0};

EulerAttitude attitude;

tPID PI_wx,PI_wy,PI_wz;
fractional abcCoefficient_wx[3] __attribute__ ( (space(xmemory),far));
fractional abcCoefficient_wy[3] __attribute__ ( (space(xmemory),far));
fractional abcCoefficient_wz[3] __attribute__ ( (space(xmemory),far));

fractional controlHistory_wx[3]  __attribute__ ( (space(ymemory),far));
fractional controlHistory_wy[3]  __attribute__ ( (space(ymemory),far));
fractional controlHistory_wz[3]  __attribute__ ( (space(ymemory),far));
fractional kCoeffs[] = {0,0,0}; // same for each axes
fractional controlOutput[] = {0,0,0}; // output PID vector

void initDCM()
{
// essai 0.25-0.05  16cp
//0.2 --- 0.05

kCoeffs[0] = 1471; 	//Q15(0.2); 	//Kp 
kCoeffs[1] =105;	// Q15(0.1); 	//Ki
kCoeffs[2] = 0; 		//Kd=0

PI_wx.abcCoefficients= &abcCoefficient_wx[0];    /*Set up pointer to derived coefficients */
PI_wx.controlHistory= &controlHistory_wx[0];     /*Set up pointer to controller history samples */
PIDInit(&PI_wx);                               /*Clear the controler history and the controller output */
PIDCoeffCalc(&kCoeffs[0], &PI_wx);             /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
PI_wx.controlReference = 0 ;  

PI_wy.abcCoefficients= &abcCoefficient_wy[0];    /*Set up pointer to derived coefficients */
PI_wy.controlHistory= &controlHistory_wy[0];     /*Set up pointer to controller history samples */
PIDInit(&PI_wx);                               /*Clear the controler history and the controller output */
PIDCoeffCalc(&kCoeffs[0], &PI_wy);             /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */
PI_wy.controlReference = 0 ;  

PI_wz.abcCoefficients= &abcCoefficient_wz[0];    /*Set up pointer to derived coefficients */
PI_wz.controlHistory= &controlHistory_wz[0];     /*Set up pointer to controller history samples */
PIDInit(&PI_wz);                               /*Clear the controler history and the controller output */
PIDCoeffCalc(&kCoeffs[0], &PI_wy);             /*Derive the a,b, & c coefficients from the Kp, Ki & Kd */	
PI_wz.controlReference = 0 ;  
}	


void gBodyEstimatorCalc(fractional *prtgBody)
{
	//TO DO
	// a faire plus tard compensation de la force centrifuge
}

void UpdatePI_TetaRollDriftComp(Dof6Accelero accel)
{
fractional error[3];
fractional gBodyEstimator[3];
//compute error beetwen DownAxe and Zbody axis
// DownAxe ,the third row of R (DCM matrix), is DownAxe of Reference frame expressed in body frame
// AcceleroVector is an approximation of g field (in fact specific force=accelero measure=a-g)
// f^DownAxe=0 if Downaxe and f (ie g if body acceleration=0) is colinear  

// step1: calc gBodyEstimator from accelerometer
gBodyEstimator[0]=accel.data[0];
gBodyEstimator[1]=accel.data[1];
gBodyEstimator[2]=accel.data[2];
gBodyEstimatorCalc(&gBodyEstimator[0]);
// step 2: calc f^DownAxe
VectorCross(&error[0],&gBodyEstimator[0],&R[6]);	
// setup PI input: input is erroe and refernce is 0 (no error ie g in body reference eand Zbody aligned)
PI_wx.measuredOutput=error[0];
PI_wy.measuredOutput=error[1];
PI_wz.measuredOutput=error[2];
// update PI for each axes 
PID(&PI_wx); 
PID(&PI_wy); 
PID(&PI_wz); 
// update global controlOutput
controlOutput[0]=PI_wx.controlOutput;
controlOutput[1]=PI_wy.controlOutput;
controlOutput[2]=PI_wz.controlOutput;
}	
				
void DCMupdate(fractional * w)
{
	VectorSubtract(3,w,w,controlOutput);// compute wgyro - gyrocompensationPIoutput
// objectif: implement DCMnew=DCMold(Imatrix+AngleSkewmatrix)

//step 1	
	// convert w rate to angle (w Q1.15)
	//k_OmegaToAngle:Q15 deltaPhiVector=Q15
	VectorScale(3,deltaPhiVector,w,k_OmegaToAngle);  
	// 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)=R*OmegaRot (identity is included in OmegaRot)

	MatrixMultiply(3,3,3,Rtemp,R,OmegaRot); // result is in Q15
	// copy Rtemp to R
	int j; 
	for (j=0; j<9;j++) R[j]=Rtemp[j];
			
	normalize();	
	
}	

void normalize(void)
//	This is the routine that maintains the orthogonality of the
//	direction cosine matrix, which is expressed by the identity
//	relationship that the cosine matrix multiplied by its
//	transpose should equal the identity matrix.
//	Small adjustments are made at each time step to assure orthogonality.
{
	fractional norm ; // actual magnitude
	fractional renorm; 
	fractional error ;	// renormalization factor
	fractional rbuff[9] ;

	//	compute -1/2 of the dot product between rows 1 and 2
	error =  - VectorDotProduct( 3 , &R[0] , &R[3] ) ; 
	error=error>>1; // to obtain error/2
	//	scale rows 1 and 2 by the error
	VectorScale( 3 , &rbuff[0] , &R[3] , error ) ; 	//-(error/2)*X term
	VectorScale( 3 , &rbuff[3] , &R[0] , error ) ;	//-(error/2)*Y term
	//	update the first 2 rows to make them closer to orthogonal:
	VectorAdd( 3 , &rbuff[0] , &rbuff[0] , &R[0] ) ; 	// Xorthogonal=X-(error/2)*X
	VectorAdd( 3 , &rbuff[3] , &rbuff[3] , &R[3] ) ;	// Yorthogonal=Y-(error/2)*Y
	//	use the cross product of the first 2 rows to get the 3rd row
	VectorCross( &rbuff[6] , &rbuff[0] , &rbuff[3] ) ; 


	// VectorPower cannot be use
	// because Q15(ie Q1.15) saturation  when addition of power terms
	norm = MyVectorPower( 3 , &rbuff[0] ); // norm is in Q2.14
	renorm=inverseSqrtNormApproximation(norm); // result is Q2.14
	VectorScale( 3 , &rbuff[0] , &rbuff[0] , renorm ) ; // Q1.15*Q2.14= Q3.29
	VectorAdd( 3 , &R[0] , &rbuff[0] ,  &rbuff[0] ) ;// x2 scaling to obtain Q15
	//	rescale row2
	norm = MyVectorPower( 3 , &rbuff[3] ); // norm is Q2.14
	renorm=inverseSqrtNormApproximation(norm);
	VectorScale( 3 , &rbuff[3] , &rbuff[3] , renorm ) ;
	VectorAdd( 3 , &R[3] , &rbuff[3] ,  &rbuff[3] ) ;// x2 scaling
	//	rescale row3
	norm = MyVectorPower( 3 , &rbuff[6] ); // norm is Q2.14
	renorm=inverseSqrtNormApproximation(norm);
	VectorScale( 3 , &rbuff[6] , &rbuff[6] , renorm ) ;
	VectorAdd( 3 , &R[6] , &rbuff[6] ,  &rbuff[6] ) ;// x2 scaling

	return ;
}


// Taylor estimation of 1/sqrt(x1^2+x2^2+x3^2) 
//INPUT in Q2.14 format
// return as Q2.14
fractional inverseSqrtNormApproximation(fractional vectnorm)
{	// Taylor estimation of 1/sqrt(x1^2+x2^2+x3^2)
	// since norm of row column are close to 1 we have  1/sqrt(1+x)
	// with x= (x1^2+x2^2+x3^2) -1
	// Taylor f=(1+x)^-0.5   f'=-(1/2).(1+x)^(3/2)    f''=(3/4)(1+x)^(5/2)
	// lim f(x->a->0) = f(a)+f'(a)(x-a) +f''(a)/2 (x-a)^2 + f'''(a)/3! (x-a)^3
	// so  1/sqrt(1+x) =1 -(1/2)x +(3/8)x^2  with x= (x1^2+x2^2+x3^2) -1
	
	long int result=0; 
	long int result2,tempo;
	fractional x=vectnorm+Q14_OneMinus; // (x1^2+x2^2+x3^2) -1
	
	// compute =-(1/2)x +(3/8)x^2
	result=__builtin_mulss(x,0x0000E000) ;// -.5= -2 +1 +0.5 =0xE000
	result2=__builtin_mulss(x,x); // x^2 term
	result2=result2>>14; // since result2 is Q2.24 scaling to Q214
	result+=__builtin_muluu(result2,0x0001800);//3/8= 0.375 *2^14= 6144
	x=result>>14; // scale Q4.24 in Q2.14
	x+=Q14_OnePlus; // add 1: 1 + [-(1/2)x +(3/8)x^2]
	
 	return (x); // return as Q2.14
}

void DCMtoEuler()
{
// all angles are coded in Q1.15
// -1.0 -> -PI  ; +0.9999 ->+PI
// careful because _Q15atanYByXByPI error become signifiant with angle>25deg

attitude.phi=Tan4Quadrant (R[7],R[8]) ; //Tan4Quadrant(y,x)
attitude.teta=- _Q15asinByPI(R[6]); 
attitude.psi=Tan4Quadrant (R[3],R[0]);
}	




/////////////////////////////////
//// MATH
fractional Tan4Quadrant(fractional y,fractional x)
{	// careful dspic function _Q15atanYByX(x,y)  with Y=sin(e) X=cos(e)	
	fractional result=0;
	if (x>0)
		if(y>0) // quadrant 1
			if (x>=y)
				{
				result=_Q15atanYByXByPI(x,y); //0->PI/4
				return (result);
				}
			else
				{
				result=16383 - _Q15atanYByXByPI(y,x); //PI/2-(angle>0)
				return (result);
				}
		else  // quadrant 4
			if (x>=_Q15abs(y))
				{
				result=_Q15atanYByXByPI(x,y); //0->-PI/4
				return (result);
				}
			else
				{
				result=-16384 - _Q15atanYByXByPI(y,x); //-PI/2-(angle<0)
				return (result);
				}
	else
		if(y>0) // quadrant 2
			if (_Q15abs(x)>=y)
				{
				result=32767+_Q15atanYByXByPI(x,y); // +PI+(angle<0)  ;
				return (result);
				}
			else
				{
				result=16383 - _Q15atanYByXByPI(y,x); //PI/2-(angle<0)
				return (result);
				}
		else
			if (x>=y)// quadrant 3 careful x <0 et y<0 , x>y => abs(x)<abs(y)
				{
				result=-16384 - _Q15atanYByXByPI(x,y); //-PI/2-(angle>0)
				return (result);
				}
			else
				{
				result=-32768 + _Q15atanYByXByPI(x,y); //-PI +(angle>0)
				return (result);
				}
				
	if ((x>0)&&(y==0)) return(0); //0
	if ((x==0)&&(y>0)) return(16383); //+PI/2
	if ((x==0)&&(y<0)) return(-16384); //-PI/2
	if ((x<0)&&(y==0)) return(32768); //-PI
	if ((x==0)&&(y==0)) return(0); //impossible	 return 0
}


//	Implement the cross product in Q15 result
void VectorCross( fractional * dest , fractional * src1 , fractional * src2 )
{
	fractional skewMatsrc1[9];
	skewMatsrc1[0]=0;
	skewMatsrc1[1]=-src1[2];
	skewMatsrc1[2]=src1[1];
	skewMatsrc1[3]=src1[2];
	skewMatsrc1[4]=0;
	skewMatsrc1[5]=-src1[0];
	skewMatsrc1[6]=-src1[1];
	skewMatsrc1[7]=src1[0];
	skewMatsrc1[8]=0;
	MatrixMultiply(3,3,1,dest,skewMatsrc1,src2);
}

// input vector in Q15 format (ieQ1.15)
// output is in Q2.14 ( a^2+b^2+c^2 might be >1!!!)
fractional MyVectorPower( int lenght, fractional *ptr )
{
long int result=0;
int i;
fractional valQ;
 
for (i=0; i<lenght;i++)
{
	valQ=ptr[i]>>1;	// scale input to Q2.14 format
	result+=__builtin_mulss(valQ,valQ) ;//
}
result=result>>14;
return (result);
}	

void EulerEstimatorAccelero(Dof6Accelero accel,EulerAttitude *ptrEuler)
{
ptrEuler->phiAccelero=Tan4Quadrant(accel.data[1],accel.data[2]);// atan(ay,az)

fractional accelDataScaled[2];
fractional norm,sqrt;
// scaling data so ay^2+az^2 cannot overflow 1
// no precision impact since accel is in fact issue from 10 bit convertion
accelDataScaled[0]=accel.data[1]>>1; // scaling /2
accelDataScaled[1]=accel.data[2]>>1; // scaling /2

norm=VectorPower(2,&(accelDataScaled[1])); // compute ax/2^2+az/2^2 result is (ay^2+az^2)/4
sqrt=_Q15sqrt(norm); // result is sqrt(ay^2+az^2)/2;
sqrt=sqrt<<1; // scale x2

// compute atan(-ax,sqrt(ay^2+az^2)
// with avoid computing sqrt(Q2.14 value) because sqrt function is compute in Q1.15
// and result is scaled with sqrt(2)!!! 
ptrEuler->tetaAccelero=Tan4Quadrant(-accel.data[0],sqrt);

}

