/*********************************************************************************
*  Embedded Studio (C) 2007                                   
*                                                                                
*  File: ES_FOC.c
*  Desc: Field Oriented Control (FOC) functions
*
**********************************************************************************/

#include "ES_FOC.h"
#include "math.h"

/*****************************************************************************
   Local Function Prototypes 
 *****************************************************************************/
SineCosineData FOC_GetPosSineCosine(FLOAT32 theta)
void FOC_TimerLimit(FLOAT32 *pVec1, FLOAT32 *pVec2, FLOAT32 *pT0);


/*********************************************************************************************
* Function: FOC_ClarkeTransform
*
* Desc: This function performs Clarke Transform to convert three phase currents
*       to alpha / beta domain currents.
*
* Params: FocThreePhaseCurrent currents
*
* Returns: FocTwoPhaseCurrent
*
* Notes: Clarke transform equations:
* 		 Isalpha = Ia
*     	 Isbeta = 1 / sqrt(3) * Ia + 2 / sqrt(3) * Ib
*		 Ia + Ib + Ic = 0
*
*********************************************************************************************/
FocTwoPhaseCurrent FOC_ClarkeTransform(FocThreePhaseCurrent currents)
{
   FocTwoPhaseCurrent clarkeCurrents;
   
   // Clarke Transform : convert 3 phase currents to alpha-beta domain currents
   clarkeCurrents.Isalpha = currents.Ia;
   clarkeCurrents.Isbeta = (currents.Ia + currents.Ib * 2) * INV_SQRT3;

   return clarkeCurrents;   
}

/*********************************************************************************************
* Function: FOC_ParkTransform
*
* Desc: This function performs Park Transform to convert two phase currents to DQ domain currents
*
* Params: FocTwoPhaseData currents
*
* Returns: FocDQData
*
* Notes: Clarke transform equations:
*     	 Isd = Isalpha * cos(theta) + Isbeta * sin(theta)
*		 Isq = -Isalpha * sin(theta) + Isbeta * cos(theta)
*
*********************************************************************************************/
FocDQCurrent FOC_ParkTransform(FocTwoPhaseCurrent currents, FLOAT32 theta)
{
   FocDQCurrent dqCurrent;
   SineCosineData posMotor;
   
   //get sine and cosine values of the motor position
   posMotor = FOC_GetPosSineCosine(theta);

   // Park Transform : phase alpha-beta frame currents to DQ frame currents
   dqCurrent.Isd = currents.Isalpha * posMotor.Cosine + currents.Isbeta * posMotor.Sine;
   dqCurrent.Isq = currents.Isbeta * posMotor.Cosine - currents.Isalpha * posMotor.Sine;
   
   return dqCurrent;
}

/*********************************************************************************************
* Function: FOC_InvParkTransform
*
* Desc: This function performs inverse Park Transform to convert DQ voltages to two phase voltages
*
* Params: FocTwoPhaseData currents
*
* Returns: FocDQData
*
* Notes: Inverse Park transform equations:
*     	 Vsalpha = Vsd * cos(theta) - Vsq * sin(theta)
*		 Vsbeta = Vsd * sin(theta) + Vsq * cos(theta)
*
*********************************************************************************************/
FocTwoPhaseVoltage FOC_InvParkTransform(FocDQVoltage dqVolts, FLOAT32 theta)
{
   FocTwoPhaseVoltage twoPhvolts;
   SineCosineData posMotor;
   
   //get sine and cosine values of the motor position
   posMotor = FOC_GetPosSineCosine(theta);

   // inverse Park Transform: phase alpha-beta frame voltages to DQ frame voltages
   twoPhvolts.Vsalpha = dqVolts.Vsd * posMotor.Cosine - dqVolts.Vsq * posMotor.Sine;
   twoPhvolts.Vsbeta  = dqVolts.Vsd * posMotor.Sine + dqVolts.Vsq * posMotor.Cosine;
   
   return twoPhvolts;
}


/*********************************************************************************************
* Function: FOC_InvClarkeTransform
*
* Desc: This function inverses Clarke transform to decodes sector number and normalized Va, Vb, Vc
*
* Params: FocTwoPhaseVoltage twoPhVolts
*
* Returns: InvClarkeData
*
* Notes: Inverse Clarke transform equations:
*     	 Va = Vsbeta
*		 Vb = (sqrt(3) * Vsalpha - Vsbeta) / 2
*		 Vc = (-sqrt(3) * Vsalpha - Vsbeta) / 2
*
*********************************************************************************************/
InvClarkeData FOC_InvClarkeTransform(FocTwoPhaseVoltage twoPhVolts)
{
    FLOAT32 halfVsbeta, halfSqrt3Vsalpha;
    INT32 sector;
    InvClarkeData invData;
    
    // compute Vsalpha / 2 = Vsalpha * 0.5
    halfVsbeta = twoPhVolts.Vsbeta * 0.5;
    // compute Vsalpha * (sqrt(3)/2) = Vsalpha * 0.8660254 
    halfSqrt3Vsalpha = twoPhVolts.Vsalpha * 0.8660254;
    
    // compute Va, Vb, Vc
    invData.Va = twoPhVolts.Vsbeta;
    invData.Vb = halfSqrt3Vsalpha - halfVsbeta;		//Vb = ((sqrt(3)Vsalpha - Vsbeta ) / 2
    invData.Vc = -halfSqrt3Vsalpha - halfVsbeta;	//Vc = (-(sqrt(3)Vsalpha - Vsbeta ) / 2
    
    //decode sector number
    sector = 0;
    
    if (invData.Va > 0.0)
    {
        sector = 1;
    }
    
    if (invData.Vb > 0.0)
    {
        sector += 2;
    }
    
    if (invData.Vc > 0.0)
    {
        sector += 4;
    }
    
    return invData;
}

/*********************************************************************************************
* Function:  FOC_GetPosSineCosine
*
* Desc: This function calculates the sine/cosine values of motor's electrical poisiton
*
* Params: float theta
*
* Returns: SineCosineData
*
* Notes: the theta is the motor's electrical postion angle in degree from a position sensor
*        angle in degrees shall be converted to radian unit in calculation
*********************************************************************************************/
SineCosineData FOC_GetPosSineCosine(FLOAT32 theta)
{
    SineCosineData data;
    
    data.Sine = sinf (theta * DEG2RAD);
    data.Cosine = cosf (theta * DEG2RAD);
    
    return data;
}

// ===========================================================================*/
//  End of file */
// ===========================================================================*/