/*****************************************************************************
 *   foc.c: FOC C file for LPC29xx Family Microprocessors
 *
 *   Copyright(C) 2007, NXP Semiconductor
 *   All rights reserved.
 *
 *   History
 *   2007.09.01  ver 1.00    Preliminary version, first Release
 *
 ****************************************************************************
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * products. This software is supplied "AS IS" without any warranties.
 * NXP Semiconductors assumes no responsibility or liability for the
 * use of the software, conveys no license or title under any patent,
 * copyright, or mask work right to the product. NXP Semiconductors
 * reserves the right to make changes in the software without
 * notification. NXP Semiconductors also make no representation or
 * warranty that such application will be suitable for the specified
 * use without further testing or modification.
 *****************************************************************************/
 
#include "LPC29xx.h"                        /* LPC29xx definitions */
#include "type.h"
#include "target.h"
#include "irq.h"
#include "extint.h"
#include "dma.h"
#include "foc.h"
#include "qei.h"
#include "pwm.h"
#include "adc.h"
#include "cordic.h"

extern volatile FOC_STRUCT FOC;	
extern volatile CALCVAR_STRUCT CALCVAR;	
extern volatile BYTE qei_index;

F16_16 theta_old;			// Previous theta which is then substracted from Theta to get
							// delta theta. This delta will be accumulated in theta_sum, and
							// after a number of accumulations Omega is calculated.
F16_16 theta_sum;			// Accumulates delta theta over a number of times
WORD theta_cnt = 0;			// Counter used to calculate motor speed. Is incremented
							// in SMC_Position_Estimation() subroutine, and accumulates
							// delta Theta. After N number of accumulations, Omega is 
							// calculated. This N is diIrpPerCalc which is defined in
							// UserParms.h.
BYTE DoSpeedPID = FALSE;	// Flag if speed PID loops have to be run 
F22_10 VqMAXfltrd;			// Filtered VqMAX for SVPWM input. Controlled by max Amp
F6_10 i_sum_flt;			// Filtered current for current limiting
F16_16 speedRamp;			// Ramp speed

// Prevent compiler warning for local union defines: 'may be used before being set'
// Solution: declare variables globally.
F6_10 sinAlpha, cosAlpha, sin_tmp, ftmp, alphaUsed, i_sum; 		  

// .full part of FIXED_6_10 fixed point variable with a step of 0.0157 rad 
const signed short F6_10_sin_lookup[101]=
{
	0, 16, 32, 48, 64, 80, 96, 112, 128, 144, 160, 176, 192, 208, 223, 239, 255, 270, 286, 301, 316, 332,
	347, 362, 377, 392, 406, 421, 436, 450, 465,479 ,493 , 507, 521, 535, 548, 562, 575, 589, 602, 615, 627, 
	640 ,652, 665, 677, 689, 701, 712, 724, 735, 746, 757, 768, 778, 789, 799, 809, 819, 828, 837, 847, 856, 
	864, 873, 881, 889, 897, 905, 912, 919, 926, 933, 940, 946, 952, 958, 963, 969, 974, 979, 983, 988, 992, 
	996, 999, 1003, 1006, 1009, 1011, 1014, 1016, 1018, 1019, 1021, 1022, 1023, 1023, 1024, 1024 }; 

/*****************************************************************************
** Function name:		FOC_Init
**
** Description:			Initialize parameters for FOC
**
** parameters:			None
** Returned value:		None 
*****************************************************************************/
void FOC_Init(void)
{
	theta_old.full = F16_16_CONST(0.0);	
	theta_sum.full = F16_16_CONST(0.0);
	
	speedRamp.full = F16_16_CONST(0.0);		// Ramp start speed = 0 rad/sec
	VqMAXfltrd.full = F22_10_CONST(0.0);	
	i_sum_flt.full = F6_10_CONST(0.0);
				
	FOC.mode = MODE_QEI;
	FOC.enabled = TRUE;
	FOC.speedEnabled = FALSE;
	FOC.uartBusy = FALSE;

	FOC.alpha = F6_10_CONST(0.0);
	FOC.sync = (0x13<<24)|(0x11<<16)|(0xEF<<8)|(0xF1<<0);	// Define synchronisation bytes 

	FOC.Vq.full = F6_10_CONST(10.0);
	FOC.Vd.full = F6_10_CONST(0.0);
	FOC.Q_int.full = F6_10_CONST(0.0);
	FOC.D_int.full = F6_10_CONST(0.0);

	// Initialize SMC
	FOC.F16_16_Valpha.full = F16_16_CONST(0.0);
	FOC.bemf_alpha.full = F16_16_CONST(0.0);
	FOC.bemf_alpha_flt.full = F16_16_CONST(0.0);
	FOC.z_alpha.full = F16_16_CONST(0.0);
	FOC.Ialpha_est.full = F16_16_CONST(0.0);
	FOC.F16_16_Vbeta.full = F16_16_CONST(0.0);
	FOC.bemf_beta.full = F16_16_CONST(0.0);
	FOC.bemf_beta_flt.full = F16_16_CONST(0.0);
	FOC.z_beta.full = F16_16_CONST(0.0);
	FOC.Ibeta_est.full = F16_16_CONST(0.0);
	FOC.F16_16_Ialpha.full = F16_16_CONST(0.0);
	FOC.Ialpha_err.full = F16_16_CONST(0.0);
	FOC.F16_16_Ibeta.full = F16_16_CONST(0.0);
	FOC.Ibeta_err.full = F16_16_CONST(0.0);
	FOC.theta.full = F16_16_CONST(0.0);
	FOC.omega.full = F16_16_CONST(0.0);
}

/*****************************************************************************
** Function name:		FOC_DefaultEEparam
**
** Description:			Calculate default parameters for FOC
**
** parameters:			pointer to parameter structure
** Returned value:		None 
*****************************************************************************/
void FOC_DefaultEEparam(PARAM_STRUCT *param)
{
	param->saved = FALSE;						
	param->currentOffset = PWM_ADCOFFSET;	
	param->Q_SP.full = F6_10_CONST(1.0);		
	param->D_SP.full = F6_10_CONST(0.0); 		
	param->Q_Kp.full = F6_10_CONST(0.30);		
	param->Q_Ki.full = F6_10_CONST(0.03);		
	param->D_Kp.full = F6_10_CONST(0.30);		
	param->D_Ki.full = F6_10_CONST(0.03);			 	
	param->speed_Kp.full = F6_10_CONST(0.4);		
	param->speed_Ki.full = F6_10_CONST(0.02);		
	param->speed_Kd.full = F6_10_CONST(0.0);		
	param->speed_SP.full = F6_10_CONST(0.7);		
	
	param->F.full = F16_16_CONST(1 - PHASE_RESISTANCE * PWMLOOPTIME / PHASE_INDUCTANCE);
	param->G.full = F16_16_CONST(PWMLOOPTIME / PHASE_INDUCTANCE);
	param->K_smc.full = F16_16_CONST(K_SMC);
	param->smc_max_err.full = F16_16_CONST(SMC_MAX_ERR);
	param->Cflt_omega.full = F16_16_CONST(OMEGA0 / PWMSPEEDRATE / 10); 		
}

/*****************************************************************************
** Function name:		FOC_loop
**
** Description:			Main FOC loop
**
** parameters:			None
** Returned value:		None 
*****************************************************************************/
void FOC_loop(void)
{
	DWORD tmpPos; 
	BYTE i, seq[] = {2, 1, 0};
	F6_10 p[3], x[3];
	F22_10 VqMAX;
	F6_10 i_err;
	static short cnt=0;
	static signed long alphaRamp; 		// Angle for speed ramp

	GPIO0_OR |= (1<<18);				// LED P0.18 on
	FOC.busy = TRUE;

	// Reconstruct currents from ADC
	// max 10 bit ADC value = fractional part of F6_10
	// 65536 = 32 Amp	
	FOC.I[FOC.adcSeq[0]].full = (((((signed long)FOC.adcVal[0]) << 21) / 32768 )+1)/2;		
	FOC.I[FOC.adcSeq[2]].full = ((-(((signed long)FOC.adcVal[1]) << 21) / 32768 )+1)/2;		
	FOC.I[FOC.adcSeq[1]].full = -FOC.I[FOC.adcSeq[0]].full - FOC.I[FOC.adcSeq[2]].full; 
	FOC.Ialpha.full = FOC.I[0].full;
	FOC.Ibeta.full = FOC.I[0].full + MULT_F6_10s(FOC.I[1],F6_10_CONST(2.0));
	FOC.Ibeta.full = MULT_F6_10s(FOC.Ibeta, F6_10_CONST(INV_SQRT3)); 

	// Determine current stator angle with estimation from virtual motor model.
	FOC.alphaEst.full = EstimateAngle();

	// Measure position	
	tmpPos=QEI_CNTS-QEIPosition; 				// invert direction
	if(tmpPos<QEI_OFFSET)tmpPos+=QEI_CNTS;
	FOC.alpha = tmpPos-QEI_OFFSET;				// 2000 ticks per rev.
	//From 0-2000 QEI ticks to 2PI radians 
	// ll = FOC.alpha << 23;
	// calcAlpha.full = ((ll/1303797)+1)/2;
	FOC.alphaCalc.full = (((((unsigned long)FOC.alpha) << 23) / 1303797 )+1)/2 +	// Preserve accuracy: ((alpha x 4) << 10) << 10. 
					 ((FOC.alpha>>9 )&1) * F6_10_CONST(1.608495439) + 	 			// If MSB-1 fell off add PI/1000 * 512  to outcome
					 ((FOC.alpha>>10)&1) * F6_10_CONST(3.216990877);				// If MSB   fell off add PI/1000 * 1024 to outcome
	
	switch(FOC.mode)
	{
		case(MODE_QEI_RAMP):
			// Do nothing
			break;		
		case(MODE_SL_RAMP):
			// Do nothing
			break;		
		case(MODE_SL):
			alphaUsed.full = FOC.alphaEst.full;
			if(DoSpeedPID)
			{
				// omega * (SPEEDLOOPFREQ * 60) / (2 * PI * 1024) = omega * 9.325485
				FOC.speed.full = (signed short)(MULT_F16_16s(FOC.omega_flt, F16_16_CONST(9.325485))>>6); 
				SpeedPID();
				DoSpeedPID = FALSE;
			}
			break;
		case(MODE_QEI):
			alphaUsed.full = FOC.alphaCalc.full;
			if(QEIINTSTAT&(1<<1))	// bit1 : TIM_int
			{
				QEICLR |= (1<<2);	// Clear bit 1 : TIM_int
				FOC.speed.full = QEIVelocity;  
				if(!QEIDirection)FOC.speed.full = -FOC.speed.full;
				SpeedPID();	
			}
			break;
		case(MODE_HALL):
			// Not implemented yet
			break;
	}

	//Transformations
	sinAlpha.full = F6_10_sin(alphaUsed.full);		// calculate sine once
	cosAlpha.full = F6_10_cos(alphaUsed.full);		// calculate cosine once
	FOC.Iq.full = MULT_F6_10(cosAlpha, FOC.Ibeta) - MULT_F6_10(sinAlpha, FOC.Ialpha);  
	FOC.Id.full = MULT_F6_10(cosAlpha, FOC.Ialpha) + MULT_F6_10(sinAlpha, FOC.Ibeta);  
	
	// Q & D PI loops 
	FOC.Q_err.full = (signed long)FOC.Q_SP.full - FOC.Iq.full;
	FOC.D_err.full = (signed long)FOC.D_SP.full - FOC.Id.full;
	FOC.Q_int.full = sl_clamp(FOC.Q_int.full + FOC.Q_err.full, CALCVAR.Q_int_max);	  
	FOC.D_int.full = sl_clamp(FOC.D_int.full + FOC.D_err.full, CALCVAR.D_int_max);
	FOC.Vq.full = ss_clamp(MULT_F22_10(FOC.Q_err, FOC.Q_Kp) + MULT_F22_10(FOC.Q_int, FOC.Q_Ki), SIGNEDSHORTMAX); 
	FOC.Vd.full = ss_clamp(MULT_F22_10(FOC.D_err, FOC.D_Kp) + MULT_F22_10(FOC.D_int, FOC.Q_Ki), SIGNEDSHORTMAX); 

	// Limit current	
	i_sum.full = Abs_F6_10(FOC.I[0]) +  Abs_F6_10(FOC.I[1]) + Abs_F6_10(FOC.I[2]);
	i_sum_flt.full = i_sum_flt.full 
					  + MULT_F6_10s(i_sum, F6_10_CONST(FILTERCOEFFICIENT_ISUM)) 
					  - MULT_F6_10s(i_sum_flt, F6_10_CONST(FILTERCOEFFICIENT_ISUM));
					    
	VqMAX.full = F22_10_CONST((32*PWM_MAX)*(32*PWM_MAX)) - MULT_F6_10(FOC.Vd, FOC.Vd);
	VqMAX.full = sqrt_F22_10(VqMAX.full);

	if(i_sum_flt.full >  F6_10_CONST(I_FLOOR) && i_sum_flt.full <  F6_10_CONST(I_TOP))
	{
		i_err.full = i_sum_flt.full - F6_10_CONST(I_FLOOR);
		// VqMax = VqMax - (i_err * ( VqMax / (I_TOP-I_FLOOR) ) );
		VqMAX.full -= MULT_F22_10s(i_err, MULT_F22_10s(VqMAX, F22_10_CONST(1.0 / (I_TOP - I_FLOOR))));
	}
	else if(i_sum_flt.full >=  F6_10_CONST(I_TOP))
	{
	   VqMAX.full = F22_10_CONST(0.1);
	}

	VqMAXfltrd.full = VqMAXfltrd.full 
					  + MULT_F22_10s(VqMAX, F22_10_CONST(FILTERCOEFFICIENT_VQMAX)) 
					  - MULT_F22_10s(VqMAXfltrd, F22_10_CONST(FILTERCOEFFICIENT_VQMAX));
	if(FOC.Vq.full > VqMAXfltrd.full)FOC.Vq.full=VqMAXfltrd.full;
	if(FOC.Vq.full < -VqMAXfltrd.full)FOC.Vq.full=-VqMAXfltrd.full;

	// Limit Vq 
	// Vd is not limited
	// Vq is limited so the vector Vs is less than a maximum of 95%.
	// The 5% left is needed to be able to measure current through shunt resistors.
	// Vs = SQRT(Vd^2 + Vq^2) < 0.95
	// Vq = SQRT(0.95^2 - Vd^2)
	VqMAX.full = F22_10_CONST((32*PWM_MAX)*(32*PWM_MAX)) - MULT_F6_10(FOC.Vd, FOC.Vd);
	VqMAX.full = sqrt_F22_10(VqMAX.full);
	if(FOC.Vq.full > VqMAX.full)FOC.Vq.full=VqMAX.full;
	if(FOC.Vq.full < -VqMAX.full)FOC.Vq.full=-VqMAX.full;

	if(FOC.mode == MODE_SL_RAMP || FOC.mode == MODE_QEI_RAMP)
	{
		// Bypass PID loops on Vq and Vd
		#define MIN_SENSORLESS_SPEED	1500  // [fractional bits]
		if(cnt++ > 750)
		{
			cnt = 0;								
			speedRamp.full++;
		}
		alphaRamp = alphaRamp + speedRamp.full;		
		if(alphaRamp > F16_16_CONST(2.0*PI))alphaRamp-=F16_16_CONST(2.0*PI);
		alphaUsed.full = alphaRamp>>6;
		
		if(FOC.mode == MODE_SL_RAMP && speedRamp.full > MIN_SENSORLESS_SPEED)FOC.mode = MODE_SL;
		if(FOC.mode == MODE_QEI_RAMP && qei_index==TRUE)FOC.mode = MODE_QEI;
				
		FOC.Vd.full = F6_10_CONST(0.0);
		FOC.Vq.full = STARTUP_TORQUE;
		sinAlpha.full = F6_10_sin(alphaUsed.full);		// calculate sine once
		cosAlpha.full = F6_10_cos(alphaUsed.full);		// calculate cosine once
	}

	// Inverse park : translate static Vq and Vd to dynamic Valpha and Vbeta
	FOC.Valpha.full = MULT_F6_10(FOC.Vd, cosAlpha) - MULT_F6_10(FOC.Vq, sinAlpha);  
	FOC.Vbeta.full = MULT_F6_10(FOC.Vd, sinAlpha) + MULT_F6_10(FOC.Vq, cosAlpha);  
   
	// Inverse Clarke
	FOC.Vr1 = FOC.Vbeta;
	// FOC.Vr2 = -FOC.Vbeta/2 + sqrt(3)/2 * FOC.Valpha;
	// FOC.Vr3 = -FOC.Vbeta/2 - sqrt(3)/2 * FOC.Valpha;
	FOC.Vr2.full = -DIV_F6_10s(FOC.Vbeta,F6_10_CONST(2.0));
	FOC.Vr3.full = FOC.Vr2.full;
	ftmp.full = MULT_F6_10s(FOC.Valpha, F6_10_CONST(SQRT3/2)); 
	FOC.Vr2.full += ftmp.full;
	FOC.Vr3.full -= ftmp.full;  
		
	// Scale from -32 - 32 to 0 - 1	
	// Dividing by 32 is same as >> 5	  
	FOC.Vr1.full >>=5; 
	FOC.Vr2.full >>=5; 
	FOC.Vr3.full >>=5; 

	//	Space vector	Switching state	On' switches 
	//	V7				[111]			1.3.5
	//	V0				[000]			2.4.6
	//	V1				[001]			2.4.5
	//	V2				[010]			2.3.6
	//	V3				[011]			2.3.5
	//	V4				[100]			1.4.6
	//	V5				[101]			1.4.5
	//	V6				[110]			1.3.6
	if( FOC.Vr1.full >= F6_10_CONST(0.0) )
	{
		if( FOC.Vr2.full >= F6_10_CONST(0.0) )			// Sector 3: (0,1,1) 0-60 degrees
		{	
			FOC.sector = 0;	
			p[2].full = F6_10_CONST(1.0)-FOC.Vr1.full-FOC.Vr2.full;
			p[2].full = DIV_F6_10s(p[2],F6_10_CONST(2.0));
			p[1].full = p[2].full+FOC.Vr1.full;
			p[0].full = p[1].full+FOC.Vr2.full;
		}
		else
		{
			if( FOC.Vr3.full >= F6_10_CONST(0.0) )		// Sector 5: (1,0,1) 120-180 degrees
			{
				FOC.sector = 2;	
				p[0].full = F6_10_CONST(1.0)-FOC.Vr3.full-FOC.Vr1.full;
				p[0].full = DIV_F6_10s(p[0],F6_10_CONST(2.0));
				p[2].full = p[0].full+FOC.Vr3.full;
				p[1].full = p[2].full+FOC.Vr1.full;
			}
			else											// Sector 1: (0,0,1) 60-120 degrees
			{
			   	FOC.sector = 1;	
				p[2].full = F6_10_CONST(1.0)+FOC.Vr3.full+FOC.Vr2.full;
				p[2].full = DIV_F6_10s(p[2],F6_10_CONST(2.0));
				p[0].full = p[2].full-FOC.Vr3.full;
				p[1].full = p[0].full-FOC.Vr2.full;
			}
		}
	}
	else
	{	
		if( FOC.Vr2.full >= F6_10_CONST(0.0) )
		{
			if( FOC.Vr3.full >= F6_10_CONST(0.0) )	 	// Sector 6: (1,1,0) 240-300 degrees
			{
				FOC.sector = 4;				
				p[1].full = F6_10_CONST(1.0)-FOC.Vr2.full-FOC.Vr3.full; 
				p[1].full = DIV_F6_10s(p[1],F6_10_CONST(2.0));
				p[0].full = p[1].full+FOC.Vr2.full;	  
				p[2].full = p[0].full+FOC.Vr3.full;
			
			}
			else											// Sector 2: (0,1,0) 300-0 degrees
			{
				FOC.sector = 5;	
				p[1].full = F6_10_CONST(1.0)+FOC.Vr1.full+FOC.Vr3.full;
				p[1].full = DIV_F6_10s(p[1],F6_10_CONST(2.0));
				p[2].full = p[1].full-FOC.Vr1.full;
				p[0].full = p[2].full-FOC.Vr3.full;
			
			}
		}
		else		 										// Sector 4: (1,0,0) 180-240 degrees
		{
			FOC.sector = 3;
			p[0].full = F6_10_CONST(1.0)+FOC.Vr1.full+FOC.Vr2.full; 
			p[0].full = DIV_F6_10s(p[0],F6_10_CONST(2.0));
			p[1].full = p[0].full-FOC.Vr2.full;
			p[2].full = p[1].full-FOC.Vr1.full;		
		}
	}

	for(i=0;i<3;i++)									  // PWM activationpoint x = pwm/2 
	{
		if(p[i].full<PWM_MIN_F6_10)p[i].full=PWM_MIN_F6_10;
		x[i].full = F6_10_CONST(0.5);
		x[i].full -= DIV_F6_10s(p[i],F6_10_CONST(2.0));
	}

	if(x[1].full<=x[0].full && x[1].full<=x[2].full)seq[0] = 1;
	if(x[0].full<=x[1].full && x[0].full<=x[2].full)seq[0] = 0;
	if(x[1].full>=x[0].full && x[1].full>=x[2].full)seq[2] = 1;
	if(x[2].full>=x[0].full && x[2].full>=x[1].full)seq[2] = 2;
	seq[1] = 3 - seq[0] - seq[2];

	if(x[seq[1]].full - x[seq[0]].full < PWM_MIN_F6_10)x[seq[0]].full = -PWM_MIN_F6_10 + x[seq[1]].full;
	if(x[seq[2]].full - x[seq[1]].full < PWM_MIN_F6_10)x[seq[2]].full = PWM_MIN_F6_10 + x[seq[1]].full;
	if(x[seq[0]].full < F6_10_CONST(0.0))
	{
		x[seq[1]].full -= x[seq[0]].full;
		x[seq[0]].full = F6_10_CONST(0.0);
	}
	
	ftmp.full = x[seq[2]].full + p[seq[2]].full;
	if(ftmp.full > F6_10_CONST(1.0))
	{        					  
		x[seq[1]].full -= F6_10_CONST(1.0);
		x[seq[1]].full -= x[seq[2]].full; 
		x[seq[1]].full -= p[seq[2]].full;
		x[seq[2]].full -= F6_10_CONST(1.0);
		x[seq[2]].full -= x[seq[2]].full;
		x[seq[2]].full -= p[seq[2]].full;
    }

	FOC.adcSeq[0] = seq[0]; // Save adc sequence
	FOC.adcSeq[1] = seq[1]; 								
	FOC.adcSeq[2] = seq[2]; 									
	
	FOC.mACT[0] = (((DWORD) x[0].part.fraction) * PWM_CYCLE)/1024 + PWM_HALFDEADTIME;
	FOC.mACT[2] = (((DWORD) x[1].part.fraction) * PWM_CYCLE)/1024 + PWM_HALFDEADTIME;
	FOC.mACT[4] = (((DWORD) x[2].part.fraction) * PWM_CYCLE)/1024 + PWM_HALFDEADTIME;
	FOC.mACT[1] = FOC.mACT[0] - PWM_DEADTIME;
	FOC.mACT[3] = FOC.mACT[2] - PWM_DEADTIME;
	FOC.mACT[5] = FOC.mACT[4] - PWM_DEADTIME;
	FOC.mDACT[0] = (((DWORD) (x[0].part.fraction + p[0].part.fraction)) * PWM_CYCLE)/1024 - PWM_HALFDEADTIME; 
	FOC.mDACT[2] = (((DWORD) (x[1].part.fraction + p[1].part.fraction)) * PWM_CYCLE)/1024 - PWM_HALFDEADTIME;
	FOC.mDACT[4] = (((DWORD) (x[2].part.fraction + p[2].part.fraction)) * PWM_CYCLE)/1024 - PWM_HALFDEADTIME; 
	FOC.mDACT[1] = FOC.mDACT[0] + PWM_DEADTIME; 
	FOC.mDACT[3] = FOC.mDACT[2] + PWM_DEADTIME;  
	FOC.mDACT[5] = FOC.mDACT[4] + PWM_DEADTIME;  	

	FOC.adcPoint[0] = FOC.mACT[(seq[1]<<1)+1] - FOC.currentOffset;
	FOC.adcPoint[1] = FOC.mACT[(seq[2]<<1)+1] - FOC.currentOffset; 
	MTIM0_MR1 = FOC.adcPoint[0];	// Match for ADC1
	MTIM0_MR2 =	FOC.adcPoint[1];	// Match for ADC2
						  
   	PWM0_Set();

	FOC.busy = FALSE;
	GPIO0_OR &= ~(1<<18);	  		// LED P0.18 off
}

/*****************************************************************************
** Function name:		SpeedPID
**
** Description:			PID control of speed
**
** parameters:			None
** Returned value:		None 
*****************************************************************************/
void SpeedPID(void)
{	
	if(!FOC.speedEnabled)return;
	FOC.speed_err.full = FOC.speed_SP.full - FOC.speed.full;				
	FOC.speed_int.full = sl_clamp(FOC.speed_int.full + FOC.speed_err.full, SIGNEDSHORTMAX*10);	
	FOC.speed_der.full = sl_clamp(FOC.speed_err.full - FOC.speed_prev_err.full, SIGNEDSHORTMAX*4);
	FOC.Q_SP.full = sl_clamp(MULT_F22_10(FOC.speed_Kp, FOC.speed_err) 
							+ MULT_F22_10(FOC.speed_Ki, FOC.speed_int) 
							+ MULT_F22_10(FOC.speed_Kd, FOC.speed_der) 
							, SIGNEDSHORTMAX );	   
	FOC.speed_prev_err.full = FOC.speed_err.full;
}

/*****************************************************************************
** Function name:		EstimateAngle
**
** Description:			Estimate angle with virtual motor algorithm using a 
**						current observer and SMC.
**
** parameters:			None
** Returned value:		estimated angle as .full part of F6_10 fixed point 
**						variable 
*****************************************************************************/	
signed short EstimateAngle(void)
{
	signed long dTheta;

	FOC.F16_16_Ialpha.full = ((signed long)FOC.Ialpha.full)<<6;
  	FOC.F16_16_Ibeta.full  = ((signed long)FOC.Ibeta.full)<<6;
    FOC.F16_16_Valpha.full = ((signed long)FOC.Valpha.full)<<6;
    FOC.F16_16_Vbeta.full  = ((signed long)FOC.Vbeta.full)<<6;
	
	// fixed point 32 = DC bus voltage 12V
	// 32/12 = 2.667; voltages have to be scaled by 1/2.667=0.375
	FOC.F16_16_Valpha.full = MULT_F16_16s(FOC.F16_16_Valpha, F16_16_CONST(0.375)); 
	FOC.F16_16_Vbeta.full = MULT_F16_16s(FOC.F16_16_Vbeta, F16_16_CONST(0.375)); 
	
	// fixed point 32 = DC bus current 20A
	// 32/20 = 1.6; voltages have to be scaled by 1/2.667=0.625
	FOC.F16_16_Ialpha.full = MULT_F16_16s(FOC.F16_16_Ialpha, F16_16_CONST(0.625)); 
	FOC.F16_16_Ibeta.full = MULT_F16_16s(FOC.F16_16_Ibeta, F16_16_CONST(0.625)); 
	
	// Calculate estimated current   
	FOC.Ialpha_est.full = MULT_F16_16(FOC.G, FOC.F16_16_Valpha) - MULT_F16_16(FOC.G, FOC.bemf_alpha)
				        - MULT_F16_16(FOC.G, FOC.z_alpha) + MULT_F16_16(FOC.F, FOC.Ialpha_est);
	FOC.Ibeta_est.full = MULT_F16_16(FOC.G, FOC.F16_16_Vbeta) - MULT_F16_16(FOC.G, FOC.bemf_beta)
				       - MULT_F16_16(FOC.G, FOC.z_beta) + MULT_F16_16(FOC.F, FOC.Ibeta_est);

	// Calculate estimation error
	FOC.Ialpha_err.full = FOC.Ialpha_est.full - FOC.F16_16_Ialpha.full;
	FOC.Ibeta_err.full = FOC.Ibeta_est.full - FOC.F16_16_Ibeta.full;

	// Calculate correction factor z
	if (Abs_F16_16(FOC.Ialpha_err) < FOC.smc_max_err.full)
	{
		// z is equal to Ialpha_err * (K_smc / smc_max_error)
		FOC.z_alpha.full = MULT_F16_16(FOC.K_smc, FOC.Ialpha_err);
		FOC.z_alpha.full = DIV_F16_16(FOC.z_alpha, FOC.smc_max_err);
	}
	else if (FOC.Ialpha_err.full > F16_16_CONST(0.0))FOC.z_alpha.full = FOC.K_smc.full; 	// upper limit to +K_smc
	else FOC.z_alpha.full = -FOC.K_smc.full;												// lower limit to -K_smc

	if (Abs_F16_16(FOC.Ibeta_err) < FOC.smc_max_err.full)
	{
		// z is equal to Ibeta_err * (K_smc / smc_max_error)
		FOC.z_beta.full = MULT_F16_16(FOC.K_smc, FOC.Ibeta_err);
		FOC.z_beta.full = DIV_F16_16(FOC.z_beta, FOC.smc_max_err);
	}
	else if (FOC.Ibeta_err.full > F16_16_CONST(0.0))FOC.z_beta.full = FOC.K_smc.full; 		// upper limit to +K_smc
	else FOC.z_beta.full = -FOC.K_smc.full;													// lower limit to -K_smc 

	// Calculate estimated BEMF voltages
	FOC.bemf_alpha.full = FOC.bemf_alpha.full + MULT_F16_16(FOC.Cflt_smc, FOC.z_alpha) 
						  - MULT_F16_16(FOC.Cflt_smc, FOC.bemf_alpha);
	FOC.bemf_beta.full = FOC.bemf_beta.full + MULT_F16_16(FOC.Cflt_smc, FOC.z_beta) 
						 - MULT_F16_16(FOC.Cflt_smc, FOC.bemf_beta);
	
	// Filter estimated BEMF voltages
	FOC.bemf_alpha_flt.full = FOC.bemf_alpha_flt.full + MULT_F16_16(FOC.Cflt_smc, FOC.bemf_alpha) 
							  - MULT_F16_16(FOC.Cflt_smc, FOC.bemf_alpha_flt);
	FOC.bemf_beta_flt.full = FOC.bemf_beta_flt.full + MULT_F16_16(FOC.Cflt_smc, FOC.bemf_beta) 
							 - MULT_F16_16(FOC.Cflt_smc, FOC.bemf_beta_flt);
	
	// Calculate theta from BEMF voltages
	FOC.theta.full = FxAtan2(FOC.bemf_beta_flt.full, -FOC.bemf_alpha_flt.full);
	dTheta = FOC.theta.full - theta_old.full;
	if(dTheta > F16_16_CONST(PI))dTheta -= F16_16_CONST(2*PI);		// Compensate for dTheta > PI
	if(dTheta < F16_16_CONST(-PI))dTheta += F16_16_CONST(2*PI);		// Compensate for dTheta < -PI
	theta_sum.full += dTheta;		 								// Add dTheta to theta sum
	theta_old.full = FOC.theta.full;								// Save old theta value
	
	if(++theta_cnt == PWMSPEEDRATE)		   							// theta_sum equals speed
	{
		FOC.omega.full = theta_sum.full;
		theta_cnt = 0;
		theta_sum.full = F16_16_CONST(0.0);
		DoSpeedPID = TRUE;											// Set Speed PID flag for FOC loop
	}
	
	// Filter omega 
    FOC.omega_flt.full = FOC.omega_flt.full + MULT_F16_16(FOC.Cflt_omega, FOC.omega) - MULT_F16_16(FOC.Cflt_omega, FOC.omega_flt);
	
	// Calculate smc filter coefficient from omega 
	FOC.Cflt_smc.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(PWMLOOPTIME/SPEEDLOOPTIME)); 
	
	// Lower limit of smc filter coefficient
	if (FOC.Cflt_smc.full < F16_16_CONST(OMEGA0 / PWMSPEEDRATE))FOC.Cflt_smc.full = F16_16_CONST(OMEGA0 / PWMSPEEDRATE);
	
	// Calculate theta compensation based on omega
	if (FOC.omega_flt.full < F16_16_CONST(OMEGA0))
	{
		FOC.theta_offset.full = F16_16_CONST(OFFSET0); 
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA1))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE0)) + F16_16_CONST(OFFSET0);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA2))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE1)) + F16_16_CONST(OFFSET1);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA3))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE2)) + F16_16_CONST(OFFSET2);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA4))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE3)) + F16_16_CONST(OFFSET3);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA5))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE4)) + F16_16_CONST(OFFSET4);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA6))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE5)) + F16_16_CONST(OFFSET5);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA7))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE6)) + F16_16_CONST(OFFSET6);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA8))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE7)) + F16_16_CONST(OFFSET7);
	}
	else if (FOC.omega_flt.full < F16_16_CONST(OMEGA9))
	{
		FOC.theta_offset.full = MULT_F16_16s(FOC.omega_flt, F16_16_CONST(SLOPE8)) + F16_16_CONST(OFFSET8);
	}
	else
	{
		FOC.theta_offset.full = F16_16_CONST(OFFSET9);
	}

	FOC.theta.full -= FOC.theta_offset.full; 										// subtract theta offset
	if(FOC.theta.full >= F16_16_CONST(PI))FOC.theta.full-=F16_16_CONST(2*PI);		// from - PI to PI
	if(FOC.theta.full < F16_16_CONST(-PI))FOC.theta.full+=F16_16_CONST(2*PI);		
	FOC.theta.full += F16_16_CONST(PI);
	if (FOC.theta.full < F16_16_CONST(0.0))FOC.theta.full+=F16_16_CONST(2*PI);		// from 0 to 2*PI
	return FOC.theta.full>>6; 
}

/*****************************************************************************
** Function name:		FOC_CalcVar
**
** Description:			Calc bounds of PI controllers for Q & D
**
** parameters:			None
** Returned value:		None 
*****************************************************************************/
void FOC_CalcVar(void)
{
 	// Calc variables used for FOC
	F6_10 tF6_10;

	// integerated error * Ki shouldnt be greater then 31.999 
	// so integrated error >= 31.999 / Ki
	tF6_10.full = 0x7FFF;	
	CALCVAR.Q_int_max = DIV_F22_10(tF6_10, FOC.Q_Ki);
	CALCVAR.D_int_max = DIV_F22_10(tF6_10, FOC.D_Ki);
}

/*****************************************************************************
** Function name:		F6_10_sin
**
** Description:			Sine calculation with lookuptable in F6_10 format
**
** parameters:			None
** Returned value:		.full part of F6_10 fixed point variable 
**
*****************************************************************************/		
signed short F6_10_sin(signed short a)
{
	BYTE pos;

	if(a < F6_10_CONST(0.0))a += F6_10_CONST(PI*2);
   	sin_tmp.full = a;
	while(sin_tmp.full > F6_10_CONST(PI*0.5))sin_tmp.full -= F6_10_CONST(PI*0.5);

	pos = (BYTE) (DIV_F6_10s(sin_tmp,F6_10_CONST(0.0157))>>10);			// 101 steps of 0.0157 rad = 1.57 rad = 90 deg		
	if(a <= F6_10_CONST(PI*0.5))									return F6_10_sin_lookup[pos];
	else if(a>F6_10_CONST(PI*0.5) && a <F6_10_CONST(PI))		return F6_10_sin_lookup[100-pos];
	else if(a>=F6_10_CONST(PI) && a < (F6_10_CONST(PI*1.5)))	return -F6_10_sin_lookup[pos];
	else 																return -F6_10_sin_lookup[100-pos];
}

/*****************************************************************************
** Function name:		F6_10_cos
**
** Description:			Cosine calculation with lookuptable in F6_10 format
**
** parameters:			None
** Returned value:		.full part of F6_10 fixed point variable 
**
*****************************************************************************/		
signed short F6_10_cos(signed short a)
{
	signed short calc;
	if(a < F6_10_CONST(0.0))a += F6_10_CONST(PI*2);
	if(a < F6_10_CONST(PI*1.5))calc = a + F6_10_CONST(PI*0.5);
	else calc = a - F6_10_CONST(PI*1.5);
	return F6_10_sin(calc);
}

/*****************************************************************************
** Function name:		sqrt_F22_10
**
** Description:			Integer squareroot of the full part of a F22_10 fixed 
**						point calculation variable 
**
** 						The integer square root of a fixed point number divided 
** 						by the square root of 2 to the F power where F is the 
** 						number of bits of fraction in the fixed point number is 
** 						the square root of the original fixed point number. 
** 						If you have an even number of bits of fraction you can 
** 						convert the integer square root to the fixed point square 
** 						root with a shift. Otherwise you have to do one divide to 
** 						recover the square root. 
**
** 						The sqrt(a * b) = sqrt(a) * sqrt(b) and
** 						fixed point numbers are scaled integers
** 						you can take the square root of a fixed 
** 						point number by taking the square root of
** 						the integer representation of the number
** 						and then dividing by the square root of the
** 						scale factor.
**
** 						For fixed point formats with an even number
** 						of fractional bits the division can be done
** 						with a simple shift operation.
**
** parameters:			.full part of F22_10 fixed point variable
** Returned value:		.full part of F22_10 fixed point variable 
**
*****************************************************************************/	
#define step(shift) \
	if((0x40000000l >> shift) + root <= value)          \
	{                                                   \
	    value -= (0x40000000l >> shift) + root;         \
	    root = (root >> 1) | (0x40000000l >> shift);    \
	}                                                   \
	else                                                \
	{                                                   \
	    root = root >> 1;                               \
	}
signed long sqrt_F22_10(signed long value)
{
    long root = 0;

    step( 0);
    step( 2);
    step( 4);
    step( 6);
    step( 8);
    step(10);
    step(12);
    step(14);
    step(16);
    step(18);
    step(20);
    step(22);
    step(24);
    step(26);
    step(28);
    step(30);

    // round to the nearest integer, cuts max error in half
	if(root < value)++root;
    root <<= 5;
	return root;
}

/*****************************************************************************
** Function name:		Abs_F6_10
**
** Description:			Absolute value of a F6_10 fixed point variable
**
** parameters:			F6_10 fixed point variable
** Returned value:		.full part of F6_10 fixed point variable 
**
*****************************************************************************/	
signed short Abs_F6_10(F6_10 value)
{
	if(value.full<0)return -value.full;
	return value.full;
}

/*****************************************************************************
** Function name:		Abs_F16_16
**
** Description:			Absolute value of a F16_16 fixed point variable
**
** parameters:			F16_16 fixed point variable
** Returned value:		.full part of F16_16 fixed point variable 
**
*****************************************************************************/
signed long Abs_F16_16(F16_16 value)
{
	if(value.full<0)return -value.full;
	return value.full;
}

/*****************************************************************************
** Function name:		ss_clamp
**
** Description:			clamp a variable to a limit
**
** parameters:			variable and limit
** Returned value:		clamped variable 
**
*****************************************************************************/
signed short ss_clamp(signed long sl, signed short clamp)
{
 	if(sl>clamp)return clamp;
	if(sl<-clamp)return -clamp;
	return (signed short)sl;
}

/*****************************************************************************
** Function name:		sl_clamp
**
** Description:			clamp a variable to a limit
**
** parameters:			variable and limit
** Returned value:		clamped variable 
**
*****************************************************************************/
signed long sl_clamp(signed long sl, signed long clamp)
{
	if(sl>clamp)return clamp;
	if(sl<-clamp)return -clamp;
	return sl;
}





    



