/*  File    : current_park.c
 *  Abstract:simulate the execution of the control software by the micro controller,debug version
 *  Copyright 2006-2007 The ITAEL of the Tongji University.
 *  $Revision: 1.0 $
 */

#define S_FUNCTION_NAME current_park
#define S_FUNCTION_LEVEL 2

#include "simstruc.h"
#include "math.h"
#include "register.h"

#define NPARAMS 0

/*user data type defines*/
typedef struct {
	int iu;
	int iv;
	int iw;
} TPhaseCurrent;

typedef struct {
	int real;
	int imag;
} TComplex;

typedef struct 
{
	int SetValue;
	int A0;					/* gain proportional Q16.0 */
	int A1;					/* gain integral Q16.0 */
	int Limit;				/* limit of the output -> saturation */
	long int Integral;		/* */
	int Error;
} TPI;

/*user macro defines*/
#define ONEOVER4	0x2000		// 0.25 in 1Q15	
#define THREEOVER4	0x6000		// 0.75 in 1Q15	
#define HALF		0x4000		// 0.5 in 1Q15	
#define TPWM 		0x7D0		// 100us equal T12P*2
#define SQRT3INV 	0x49E7		// 1/sqrt(3)=0.57735
#define SQRT2INV	0x5A82		// 1/sqrt(2)=0.7071 		
#define ENC_OFFSET	0x0000		// 0 = Zero Position of PMSM
#define LEM_OFFSET      0x0200		// 512 equals 2.5V on ADC Input

#define	ScaleQ15(Value) (Value & 0x3FF) << 5   //scaling for AD sampled value
#define	ScaleQ15Neg(Value) (- (Value & 0x3FF)) << 5
/*user global variables and constants*/
const int T_sample = 0x000D;  /* 400us  Q1.15 */
char dir_flag=1;
int speed = 50 << 3;//set point of the rotor speed
char Sector = 0;
unsigned int adc_result[3];
int period_number=1;
int count=0;
int QuadratureCurrent = 0x0;
int DirectCurrent  = 0x0;
int RotorSpeedSet = 0x0;
int RotorSpeed;
int oldposition;
int deltaposition;
int deltagammaQ15=0;	
int omega;
int overflow_flag=0;
char speed_loop_hit=0;
unsigned int decoval;
unsigned long int decoder_wert;
int olddeltagamma=0;

TComplex u_s;//got to be defined as global variable to debug the programm
TPhaseCurrent PhaseCurrentQ15;
TComplex Gamma;
TComplex RotorCurrentQ15;
TComplex PIRotorCurrentQ15;
TComplex StatorCurrentQ15;

TPI PI_Data_Id;		// PI Current Direct
TPI PI_Data_Iq;		// PI Current Quadrature
TPI PI_Data_Speed;	// PI control speed
/*user function prototypes*/
void InitPIController(TPI * Data, int Kp, int Ki,int Limit);
void InitFOC(void);
void Pmsm_Foc(void);
void Iphases(void);
int High(long int a);
int position=0;//for debugging purpose
void PI_Controller(TPI * PI, int NewInput, int * Result);
int GetRotorPosition(void);
void ClarkeParkTrans(TPhaseCurrent *In, TComplex *angle, TComplex *Result);
void ParkInverse (TComplex *Frame_dq, TComplex *angle, TComplex  *Result);
int GetSpeed(int actpos);
void SVM(TComplex  *m, char *Sector);
/*====================*
 * S-function methods *
 *====================*/
/* Function: mdlInitializeSizes ===============================================
 * Abstract:
 *    The sizes information is used by Simulink to determine the S-function
 *    block's characteristics (number of inputs, outputs, states, etc.).
 */
static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, NPARAMS);  /* Number of expected parameters */

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 3)) return;
    /*stator phase u current*/
    ssSetInputPortWidth(S, 0, 1);  
    /*stator phase v current*/
    ssSetInputPortWidth(S, 1, 1);
    /*contend of the result register of the timer which is in the incremental encoder interface mode*/
    ssSetInputPortWidth(S, 2, 1);
    /*define input port data types*/
    ssSetInputPortDataType(S, 0, SS_INT16);
    ssSetInputPortDataType(S, 1, SS_INT16);
    ssSetInputPortDataType(S, 2, SS_INT16);
    /*set direct feedthru appearing in the system output*/
    ssSetInputPortDirectFeedThrough(S, 0, 1);
    ssSetInputPortDirectFeedThrough(S, 1, 1);
    ssSetInputPortDirectFeedThrough(S, 2, 1);
    /*model output are compare values of the pwm channel*/
    if (!ssSetNumOutputPorts(S, 4)) return;
    
    ssSetOutputPortWidth(S, 0, 1);
    ssSetOutputPortWidth(S, 1, 1);
    ssSetOutputPortWidth(S, 2, 1);
    ssSetOutputPortWidth(S, 3, 1);
       
    ssSetNumSampleTimes(S, 1);  
    /*user defined initialization*/
    InitFOC();	
    /* Take care when specifying exception free code - see sfuntmpl_doc.c */
    ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    S-function is comprised of only continuous sample time elements
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, 0.0001);
    ssSetOffsetTime(S, 0, 0.0);
}

/* Function: mdlOutputs =======================================================
 * Abstract:
 *      simulate the mcu software
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    real_T	      time = ssGetT(S);
    
    real_T            *y0       = ssGetOutputPortRealSignal(S,0);
    real_T            *y1       = ssGetOutputPortRealSignal(S,1);
    real_T            *y2       = ssGetOutputPortRealSignal(S,2);
    real_T            *y3       = ssGetOutputPortRealSignal(S,3);
       
    InputPtrsType       u_0     = ssGetInputPortSignalPtrs(S,0);
    InputInt16PtrsType  i_u     = (InputInt16PtrsType)u_0;    
    InputPtrsType       u_1     = ssGetInputPortSignalPtrs(S,1);
    InputInt16PtrsType  i_v     = (InputInt16PtrsType)u_1;    
    InputPtrsType       u_2     = ssGetInputPortSignalPtrs(S,2);
    InputInt16PtrsType  GPT_Tx  = (InputInt16PtrsType)u_2;    
               
    adc_result[0] = *i_u[0];
    adc_result[1] = *i_v[0];
    GPT12E_T3 = *GPT_Tx[0];
            
    Pmsm_Foc();
 	   
    y0[0] = RotorCurrentQ15.real;
    y1[0] = RotorCurrentQ15.imag;    
    y2[0] = Gamma.real;
    y3[0] = Gamma.imag;
}
/* Function: InitPIController=======================================================
 * Abstract:
 *    implements initialization of the PI controllers
 */
void InitPIController(TPI * Data, int Kp, int Ki,int Limit)
{	
	Data->SetValue = 0;
	Data->A0 = Kp;
	Data->A1 = ((((long)Ki << 4)*(long)T_sample) >> 15) - Kp; /* Q16.0 format */
	Data->Limit = Limit;
	Data->Integral = 0;
	Data->Error = 0;
}
/* Function: InitPIController=======================================================
 * Abstract:
 *    initialize the whole control software
 */
void InitFOC(void)
{	
	InitPIController(&PI_Data_Speed, 0x3000, 0x0020, 0x5000);
	InitPIController(&PI_Data_Id, 0x0008, 0x0008,0x5555);
	InitPIController(&PI_Data_Iq, 0x0008, 0x0008,0x5555);

	DirectCurrent	=	0x0000;		// no field-weaking
	QuadratureCurrent=	0x051E;
}
/* Function: PI_Controller==========================================================
 * Abstract:
 *    implements a digital PI controller algorithm
 */
void PI_Controller(TPI * PI, int NewInput, int * Result)
{
	int NewError;
	NewError = PI->SetValue - NewInput;
 	//PI->Integral = PI->Integral + PI->A0*NewError + PI->A1*PI->Error;
	PI->Integral = (long)PI->A0*(long)NewError;
	PI->Error = NewError;
	if (PI->Integral>PI->Limit ) 
		*Result=PI->Limit;
		else if (PI->Integral<-PI->Limit )
			*Result=-PI->Limit;
			else
		        *Result = PI->Integral;
		}

int High(long int a)
{
	return a >> 15;
}
			
/* Function: PMSM_FOC==========================================================
 * Abstract:
 *    executed every Ts(100us)
 */
void Pmsm_Foc(void)
{
	int actpos;
	int position;
	
	switch (period_number)
	{
	case 1:
		Iphases();			// Get 3 phase currents from lem current transducers
		position=GetRotorPosition();	// Get new rotor position in terms of electrical angle
		/*if (count >= 22)				// execute the speed controller each 4.2ms
		{
			actpos=position;
			RotorSpeed=GetSpeed(actpos);
			if (dir_flag)
			{
				RotorSpeedSet=ScaleQ15(speed);
			}
			else
			{
				RotorSpeedSet=ScaleQ15Neg(speed);
			}
			PI_Data_Speed.SetValue = RotorSpeedSet;	
			PI_Controller(&PI_Data_Speed, RotorSpeed, &QuadratureCurrent);
			count=0;					
		}
		count++;
		if( RotorSpeedSet == 0 )
		{
			QuadratureCurrent = 0;
			DirectCurrent = 0;
		}*/
		// Execute the clarke & park transformation
		ClarkeParkTrans(&PhaseCurrentQ15, &Gamma ,&RotorCurrentQ15);

		period_number=2; 
	break;
		
	case 2: 
		// Execute the two PI controller for the current
		PI_Data_Id.SetValue = DirectCurrent;
		PI_Data_Iq.SetValue = QuadratureCurrent;
		PI_Controller(&PI_Data_Id, RotorCurrentQ15.real,&PIRotorCurrentQ15.real);								// 12,8祍
		PI_Controller(&PI_Data_Iq, RotorCurrentQ15.imag,&PIRotorCurrentQ15.imag);								// 12,8祍
		//Execute the reverse park transformation
		ParkInverse(&PIRotorCurrentQ15, &Gamma, &StatorCurrentQ15);
		SVM(&StatorCurrentQ15, &Sector);

		period_number=1; 
		break;

	default: period_number=1;
	}								
}

/* Function: Idc_phases========================================================
 * Abstract:
 *    performs phase currents reconstruction according to the sector number
 */
void Idc_phases(int Sector)
{
	
}
/* Function: Iphases===========================================================
 * Abstract:
 *    performs phase currents acquirement with LEM sensors
 */
void Iphases (void) 
{
	int adciu;
	int adciv;

	// Set the ADC and start the conversion
	ADC_CON_ADST   =  1;           	// set start bit for phases measurement

	adciu=adc_result[0]&0x03FF;		
	adciu=adciu-LEM_OFFSET;
	adciv=adc_result[1]&0x03FF;		
	adciv=adciv-LEM_OFFSET;
	
	if (adciu>0)
	{
		PhaseCurrentQ15.iu =ScaleQ15(adciu);
	}
	else
	{
		adciu=-adciu;
		PhaseCurrentQ15.iu =ScaleQ15Neg(adciu);
	}
	if (adciv>0)
	{
		PhaseCurrentQ15.iv =ScaleQ15(adciv);
	}
	else
	{
		adciv=-adciv;
		PhaseCurrentQ15.iv =ScaleQ15Neg(adciv);
	}
	PhaseCurrentQ15.iw = -PhaseCurrentQ15.iu - PhaseCurrentQ15.iv;
}

/* Function: GetRotorPosition===================================================
 * Abstract:
 *    acquires rotor position value from the position sensor，which is in this 
 *    case an optical encoder connected to the GPT of the MCU 
 */
int GetRotorPosition(void)
{
	int decoval;

	decoval=GPT12E_T3;		
	if(!(decoval>=0&&decoval<=2000))		
		decoval=decoval-0xF830;	//if motor starts in the negative direction		

	decoval=decoval%1000;
	decoval=decoval+ENC_OFFSET;	
	
	/*if (decoval>1000)
	{
		decoval=decoval-1000;
	}*/

	// Sinus and Cosine Function of rotor electrical angle
	// for Park Transformation
	Gamma.imag=sin_table[decoval];
	Gamma.real=cos_table[decoval];
		
	return decoval;
}

/* Function: ClarkParkTrans==================================================
 * Abstract:
 *    performs clark and park transformation
 */ 
void ClarkeParkTrans(TPhaseCurrent *In, TComplex *angle, TComplex *Result)
{
	int Ibeta;
	TComplex IsPark;

	Ibeta = ((long)In->iv + (long)In->iv +(long)In->iu)*(long)SQRT3INV >> 15;
	IsPark.real = ((long)In->iu)*((long)angle->real) + ((long)Ibeta)*((long)angle->imag) >> 15;
	IsPark.imag = ((long)Ibeta)*((long)angle->real) - ((long)In->iu)*((long)angle->imag) >> 15;

	* Result =IsPark; 
 
}
/* Function: InverseParkTrans==================================================
 * Abstract:
 *    performs inverse park transformation
 */
void ParkInverse (TComplex *Frame_dq, TComplex *angle, TComplex  *Result)
{
	TComplex Is_rev_Park;

	Is_rev_Park.real = ((long)Frame_dq->real)*((long)angle->real) - ((long)Frame_dq->imag)*((long)angle->imag) >> 15;
	Is_rev_Park.imag = ((long)Frame_dq->real)*((long)angle->imag) + ((long)Frame_dq->imag)*((long)angle->real) >> 15;

	*Result=Is_rev_Park;
 }
/* Function: GetSpeed==================================================
 * Abstract:
 *    performs inverse park transformation
 */
int GetSpeed(int actpos)
{
	//int olddeltagamma;
	
	olddeltagamma=deltagammaQ15;

	deltaposition=actpos-oldposition;

	if (deltaposition>500) 	// overflow in left direction?
	{
		deltaposition= deltaposition-1000;
	}
	if (deltaposition < (-250))  // overflow in right direction?
	{
		deltaposition= deltaposition+1000;
	}

	if (deltaposition>=0)	// direction?
	{
		deltagammaQ15=(deltaposition & 0xFF)<<7;
	}
	else 
	{
		deltagammaQ15= 0x8000 + ((deltaposition & 0xFF)<<7);
	}
	omega=deltagammaQ15;

	deltagammaQ15=(((long)omega*(long)ONEOVER4)+((long)olddeltagamma*(long)THREEOVER4))>>15;

	oldposition=actpos;
	return deltagammaQ15;
}

/* Function: SVM==================================================
 * Abstract:
 *    implements the space vector modulation
 */
void SVM(TComplex  *m, char *Sector)					
{
	int x;
	unsigned int to,tk,tk1;
	// determination of the sector
	x=((long)m->imag*(long)SQRT3INV) >> 15;	
	if (m->imag>0)
	{	// m.imag>0
		if (m->real>0)
		{	// Quadrant 1
			if (x<m->real) {*Sector=0;} else {*Sector=1;}
		}
		else
		{	// Quadrant 2
			if (x< -m->real) {*Sector=2;} else {*Sector=1;}
		}
	}
	else
	{	// m.imag<0
		if (m->real<0)
		{	// Quadrant 3
			if (x<m->real)	{*Sector=4;} else	{*Sector=3;}
		}
		else
		{	// Quadrant 4
			if (x< -m->real)  {*Sector=4;} else	{*Sector=5;}
		}
	}    
	//calculate the compare value needed to generate the pwm signals
	
	tk1=((long)CosVectorQ1_15[*Sector]*(long)m->imag - (long)SinVectorQ1_15[*Sector]*(long)m->real) >> 15;
	tk1=((long)tk1 * (long)TPWM) >> 15;

	tk=((long)SinVectorQ1_15[*Sector+1]*(long)m->real - (long)CosVectorQ1_15[*Sector+1]*(long)m->imag) >> 15;
	tk=((long)tk * (long)TPWM) >> 15;

	to=1001-tk-tk1;						

	// Write t0, tk and tk1 to the shadow registers of CC6
	// Set Timer 13 Event Control, for ADC start
	switch (*Sector)
	{
	case 0:
		CCU6_CC60SR=to>>2;
		CCU6_CC61SR=500-(to>>2)-(tk1>>1);
		CCU6_CC62SR=500-(to>>2);
		break;	
	case 1:
		CCU6_CC60SR=(to>>2)+(tk1>>1);
		CCU6_CC61SR=to>>2;
		CCU6_CC62SR=500-(to>>2);
		break;	
	case 2:
		CCU6_CC60SR=500-(to>>2);
		CCU6_CC61SR=to>>2;
		CCU6_CC62SR=500-(to>>2)-(tk1>>1);
		break;	
	case 3:
		CCU6_CC60SR=500-(to>>2);
		CCU6_CC61SR=(to>>2)+(tk1>>1);
		CCU6_CC62SR=to>>2;
		break;	
	case 4:
		CCU6_CC60SR=500-(to>>2)-(tk1>>1);
		CCU6_CC61SR=500-(to>>2);
		CCU6_CC62SR=to>>2;
		break;	
	case 5:
		CCU6_CC60SR=(to>>2);
		CCU6_CC61SR=500-(to>>2);
		CCU6_CC62SR=(to>>2)+(tk1>>1);
		break;	
	}

}

/* Function: mdlTerminate =====================================================
 * Abstract:
 *    No termination needed, but we are required to have this routine.
 */
static void mdlTerminate(SimStruct *S)
{
    
}

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif