/******************************************************************************
* 
* Copyright (c) 2013 Freescale Semiconductor;
* All Rights Reserved                       
*
******************************************************************************* 
*
* THIS SOFTWARE IS PROVIDED BY FREESCALE "AS IS" AND ANY EXPRESSED OR 
* IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
* OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  
* IN NO EVENT SHALL FREESCALE OR ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
* INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) 
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, 
* STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
* IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF 
* THE POSSIBILITY OF SUCH DAMAGE.
*
***************************************************************************//*!
*
* @file      motor_structure.c
*
* @author    R61928, R65119
* 
* @version   1.0.5.0
* 
* @date      Nov-07-2013
* 
* @brief     Motor control structure
*
*******************************************************************************
*
* Motor control structure.
*
******************************************************************************/

/******************************************************************************
* Includes
******************************************************************************/
#include "motor_structure.h"

/******************************************************************************
* Constants
******************************************************************************/

/******************************************************************************
* Macros 
******************************************************************************/

/******************************************************************************
* Types
******************************************************************************/


/******************************************************************************
* Global variables
******************************************************************************/

/******************************************************************************
* Global functions
******************************************************************************/
/***************************************************************************//*!
*
* @brief   PMSM field oriented control
*
* @param   MCSTRUC_FOC_PMSM_T *psFocPMSM
*			- structure of PMSM FOC parameters
*			IN:->sIABC - input ABC phases currents
*			IN:->sAnglePosEl - angle where the currents were measured
*			IN:->sAnglePosElUpdate - angle where the next PWM reload
*			IN:->f16UDcBusFilt - DC bus voltage
*			IN:->bUseMaxBus - true to calculate max. possible output DQ voltage
*					limits in dependence on the dc bus voltage. False to keep the
*					output DQ voltage limits intact.
*			IN:->f16DutyCycleLimit - determines the max. value of duty cycle
*			IN/OUT->sIdPiParams - D current controller structure
*			IN/OUT->sIqPiParams - Q current controller structure
*			IN/OUT->i16IdPiSatFlag - D current controller saturation flag
*			IN/OUT->i16IqPiSatFlag - Q current controller saturation flag
*			OUT->sDutyABC - ABC duty cycles
*			OUT->uw16SectorSVM - Next step SVM sector
*
* @return  N/A
*
******************************************************************************/
void MCSTRUC_FocPMSMCurrentCtrl(MCSTRUC_FOC_PMSM_T *psFocPMSM)
{
	/* 3-phase to 2-phase transformation to stationary ref. frame */
	GMCLIB_Clark_F16(&psFocPMSM->sIABC, &psFocPMSM->sIAlBe);
	
	/* 2-phase to 2-phase transformation to rotary ref. frame */
	GMCLIB_Park_F16(&psFocPMSM->sIAlBe, &psFocPMSM->sAnglePosEl, &psFocPMSM->sIDQ);


	if (psFocPMSM->bUseZc)
	{
	  	/* Zero cancellation filters for the D, Q current components */
	   	psFocPMSM->sIDQReqZc.f16D = GDFLIB_FilterIIR1_F16(psFocPMSM->sIDQReq.f16D, &psFocPMSM->sIdZcFilter);
	   	psFocPMSM->sIDQReqZc.f16Q = GDFLIB_FilterIIR1_F16(psFocPMSM->sIDQReq.f16Q, &psFocPMSM->sIqZcFilter);

		/* D current error calculation */
	    psFocPMSM->sIDQError.f16D = sub(psFocPMSM->sIDQReqZc.f16D, psFocPMSM->sIDQ.f16D);
	
		/* Q current error calculation */
		psFocPMSM->sIDQError.f16Q = sub(psFocPMSM->sIDQReqZc.f16Q, psFocPMSM->sIDQ.f16Q);

	}
	else
	{
		/* D current error calculation */
	    psFocPMSM->sIDQError.f16D = sub(psFocPMSM->sIDQReq.f16D, psFocPMSM->sIDQ.f16D);

		/* Q current error calculation */
		psFocPMSM->sIDQError.f16Q = sub(psFocPMSM->sIDQReq.f16Q, psFocPMSM->sIDQ.f16Q);
	}
	
    /*** D - controller ***/
		
	if (psFocPMSM->bUseMaxBus)
	{
    	psFocPMSM->sIdPiParams.f16UpperLim = mult_r(psFocPMSM->f16DutyCycleLimit, psFocPMSM->f16UDcBusFilt);
    	psFocPMSM->sIdPiParams.f16LowerLim = negate(psFocPMSM->sIdPiParams.f16UpperLim);
	}

    /* D current PI controller */
    psFocPMSM->sUDQController.f16D = GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16D, &psFocPMSM->bIdPiSatFlag, &psFocPMSM->sIdPiParams);

	/* D current controller saturation flag */
	psFocPMSM->bIdPiSatFlag = psFocPMSM->sIdPiParams.bLimFlag;


    /*** Q - controller ***/
	if (psFocPMSM->bUseMaxBus)
	{
   		psFocPMSM->sIqPiParams.f16UpperLim = GFLIB_Sqrt_F16l(L_sub( \
   		L_mult(psFocPMSM->sIdPiParams.f16UpperLim, psFocPMSM->sIdPiParams.f16UpperLim), \
    	L_mult(psFocPMSM->sUDQController.f16D, psFocPMSM->sUDQController.f16D)));
        
        psFocPMSM->sIqPiParams.f16LowerLim = negate(psFocPMSM->sIqPiParams.f16UpperLim);
	}



    /* Q current PI controller */
    psFocPMSM->sUDQController.f16Q = GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16Q, &psFocPMSM->bIqPiSatFlag, &psFocPMSM->sIqPiParams);

	/* Q current controller saturation flag */
	psFocPMSM->bIqPiSatFlag = psFocPMSM->sIqPiParams.bLimFlag;

	if (!psFocPMSM->bOpenLoop)
	{
		/* D, Q voltage application */
		psFocPMSM->sUDQReq.f16D = psFocPMSM->sUDQController.f16D;
    	psFocPMSM->sUDQReq.f16Q = psFocPMSM->sUDQController.f16Q;	
	}
    
 	/* 2-phase to 2-phase transformation to stationary ref. frame */
	GMCLIB_ParkInv_F16(&psFocPMSM->sUDQReq, &psFocPMSM->sAnglePosElUpdate, &psFocPMSM->sUAlBeReq);

    /* Begin - voltage control */
    GMCLIB_ElimDcBusRipFOC_F16(psFocPMSM->f16UDcBusFilt, &psFocPMSM->sUAlBeReq, &psFocPMSM->sUAlBeComp);
    
    psFocPMSM->uw16SectorSVM = GMCLIB_SvmStd_F16(&psFocPMSM->sUAlBeComp, &psFocPMSM->sDutyABC);
    /* End - voltage control */	
}


/***************************************************************************//*!
*
* @brief   Position from the encoder
*
* @param   MCSTRUC_POS_SPEED_ENCODER_T *psPosition
*			- structure of encoder parameters
*			IN:->uw16PositionMechRaw - raw position fetched from encoder
*			IN:->uw16PositionMechRawMask - mask to filter the only range from
*				the position information
*			IN:->f16PositionMechScale - scale for the mechanical position
*			IN:->w16PositionMechScaleShift - scale shift for the mechanical position
*			IN:->w16PolePairs - number of pole-pairs
*			OUT:->f16PositionMech - mechanical position
*			IN/OUT:->f16PositionEl - electrical position in the instant when
*					fetched from the encoder
*			OUT:->f16PositionElUpdate - electrical position predicted for
*					the time of PWM update (adding the difference between two
*					encoder reading states)	
*			OUT:->sAnglePosEl - angle at the instant when fetched from the encoder
*			OUT:->sAnglePosElUpdate - predicted angle for the PWM update
*
* @return  N/A
*
******************************************************************************/
void MCSTRUC_AngleFromEncoder(MCSTRUC_POS_SPEED_ENCODER_T *psAngle)
{
	Frac16 f16Pos;
	Frac32 f32Pos;
	Frac16 f16OldPos;

	f16OldPos = psAngle->f16PositionEl;

	f16Pos = (Frac16)(psAngle->uw16PositionMechRaw & psAngle->uw16PositionMechRawMask);
	 
	f32Pos = L_mult(f16Pos, psAngle->f16PositionMechScale);

	psAngle->f16PositionMech = extract_h(f32Pos << psAngle->w16PositionMechScaleShift);
	
	psAngle->f16PositionEl = psAngle->f16PositionMech * psAngle->w16PolePairs;
	
	psAngle->f16PositionElUpdate = psAngle->f16PositionEl + psAngle->f16PositionEl - f16OldPos;

	/* Angle calculation */
   	psAngle->sAnglePosEl.f16Sin = GFLIB_Sin_F16(psAngle->f16PositionEl);
	psAngle->sAnglePosEl.f16Cos = GFLIB_Cos_F16(psAngle->f16PositionEl);

   	psAngle->sAnglePosElUpdate.f16Sin = GFLIB_Sin_F16(psAngle->f16PositionElUpdate);
	psAngle->sAnglePosElUpdate.f16Cos = GFLIB_Cos_F16(psAngle->f16PositionElUpdate);

}



/***************************************************************************//*!
*
* @brief   4-channel phase current calibration (calibration at app init)
*
* @param   MCSTRUC_ADC_CURRENT_CH_T *psCurrent
*			- measured current channels
*
*		   MCSTRUC_ADC_OFFSET_T *psOffset
*			- channels offset
*
* @return  N/A
*
******************************************************************************/
void MCSTRUC_Current4ChannelCalibration(MCSTRUC_ADC_CURRENT_CH_T *psCurrent, MCSTRUC_ADC_OFFSET_T *psOffset)
{
	psOffset->sI.f16Ph0 = GDFLIB_FilterMA_F16(psCurrent->f16Ph0, &psOffset->sPh0);//IA @ANAx
    psOffset->sI.f16Ph1 = GDFLIB_FilterMA_F16(psCurrent->f16Ph1, &psOffset->sPh1);//IB @ANBx
    
    psOffset->sI.f16Ph2 = GDFLIB_FilterMA_F16(psCurrent->f16Ph2, &psOffset->sPh2);//IC @ANAx
    psOffset->sI.f16Ph3 = GDFLIB_FilterMA_F16(psCurrent->f16Ph3, &psOffset->sPh3);//IC @ANBx
}

/***************************************************************************//*!
*
* @brief   2-channel phase current calibration (calibration at motor running)
*
* @param   UWord16 uw16SVMSectorParam
*			- SVM sector
*
*		   MCSTRUC_ADC_CURRENT_CH_T *psCurrent
*			- measured current channels
*
*		   MCSTRUC_ADC_OFFSET_T *psOffset
*			- channels offset
*
* @return  N/A
*
******************************************************************************/
void MCSTRUC_Current2ChannelUpdateOffset(UWord16 uw16SVMSectorParam, GFLIB_VECTORLIMIT_T_F16 *psCurrent, MCSTRUC_ADC_OFFSET_T *psOffset)
{
	/*
 	 * 	ph0: ANA_I_A
 	 * 	ph1: ANB_I_B
 	 * 	ph2: ANA_I_C
 	 * 	PH3: ANB_I_C
 	 * */
	switch (uw16SVMSectorParam) 
    {
	    case 2:
	    case 3: 
	    		{	//direct sensing of A, C, calculation of B 
					psOffset->sI.f16Ph0 = GDFLIB_FilterMA_F16(psCurrent->f16A, &psOffset->sPh0);//IA
				    psOffset->sI.f16Ph3 = GDFLIB_FilterMA_F16(psCurrent->f16B, &psOffset->sPh3);//IC
				return;
		       	}
		       	break;
		       
	    case 4: 
	    case 5: 
	    		{	//direct sensing of A, B, calculation of C 
					psOffset->sI.f16Ph0 = GDFLIB_FilterMA_F16(psCurrent->f16A, &psOffset->sPh0);//IA
				    psOffset->sI.f16Ph1 = GDFLIB_FilterMA_F16(psCurrent->f16B, &psOffset->sPh1);//IB
				return;
		        }		        
	            break;
	    case 1:
	    case 6:
	    default:
		        {	//direct sensing of C, B, calculation of A  
					psOffset->sI.f16Ph2 = GDFLIB_FilterMA_F16(psCurrent->f16A, &psOffset->sPh2);//IC
				    psOffset->sI.f16Ph1 = GDFLIB_FilterMA_F16(psCurrent->f16B, &psOffset->sPh1);//IB
	            return;
		        }
	    break;
    }
}

/***************************************************************************//*!
*
* @brief   PMSM Alignment with rotation
*
* @param   MCSTRUC_FOC_PMSM_T *psFocPMSM
*			- structure of PMSM FOC parameters
*			IN/OUT:->sAlignment.f32Position - position of the field
*			IN:->sAlignment.f32Speed - speed of the field
*			IN:->sIDQ.f16D - measured D current
*			IN:->sAlignment.f16UMax - max D voltage at alignment
*			IN:->sAlignment.f16IMax - max D current at alignment
*			IN/OUT:->sAlignment.f32U - alignment D voltage which is ramped
*			IN:->sAlignment.f32UStep - voltage step to ramp the voltage
*			OUT:->psFocPMSM->sDutyABC - duty cycles ABC
*
*
* @return  N/A
*
******************************************************************************/
void MCSTRUC_AlignmentPMSM(MCSTRUC_FOC_PMSM_T *psFocPMSM)
{
	GMCLIB_2COOR_SINCOS_T_F16 sAngle;

	sAngle.f16Sin = GFLIB_Sin_F16(extract_h(psFocPMSM->sAlignment.f32Position));
	sAngle.f16Cos = GFLIB_Cos_F16(extract_h(psFocPMSM->sAlignment.f32Position));

  	/* 3-phase to 2-phase transformation to stationary ref. frame */
	GMCLIB_Clark_F16(&psFocPMSM->sIABC, &psFocPMSM->sIAlBe);

	/* 2-phase to 2-phase transformation to rotary ref. frame */
	GMCLIB_Park_F16(&psFocPMSM->sIAlBe, &sAngle, &psFocPMSM->sIDQ);

#if ALIGN_METHOD == 0
	/* Ramp D voltage until it reaches f16MaxVoltage or current reaches f16MaxCurrent */
	if ((psFocPMSM->sUDQReq.f16D < psFocPMSM->sAlignment.f16UMax) && (psFocPMSM->sIDQ.f16D < psFocPMSM->sAlignment.f16IMax))
	{
		/* 32 frac calculation because the step is smaller than 16-bit LSB */
		psFocPMSM->sAlignment.f32U += psFocPMSM->sAlignment.f32UStep;
		
		/* Copies the desired voltage to the system */
		psFocPMSM->sUDQReq.f16D = extract_h(psFocPMSM->sAlignment.f32U);		
	}
	
	psFocPMSM->sUDQReq.f16Q = FRAC16(0.0);
#else
	if (psFocPMSM->sAlignment.f16Id < psFocPMSM->sAlignment.f16IMax)
	{
		/* 32 frac calculation because the step is smaller than 16-bit LSB */
		psFocPMSM->sAlignment.f32Id += psFocPMSM->sAlignment.f32IdStep;
		
		/* Copies the desired current to the system */
		psFocPMSM->sAlignment.f16Id = extract_h(psFocPMSM->sAlignment.f32Id);
	}
	
	psFocPMSM->sIDQReq.f16D = psFocPMSM->sAlignment.f16Id;
	psFocPMSM->sIDQReq.f16Q = FRAC16(0.0);
	
	/* D current error calculation */
    psFocPMSM->sIDQError.f16D = sub(psFocPMSM->sIDQReq.f16D, psFocPMSM->sIDQ.f16D);

	/* Q current error calculation */
	psFocPMSM->sIDQError.f16Q = sub(psFocPMSM->sIDQReq.f16Q, psFocPMSM->sIDQ.f16Q);
	
    /* D current PI controller */
    psFocPMSM->sUDQController.f16D = GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16D, &psFocPMSM->bIdPiSatFlag, &psFocPMSM->sIdPiParams);

	/* D current controller saturation flag */
	psFocPMSM->bIdPiSatFlag = psFocPMSM->sIdPiParams.bLimFlag;

    /* Q current PI controller */
    psFocPMSM->sUDQController.f16Q = GFLIB_CtrlPIpAW_F16(psFocPMSM->sIDQError.f16Q, &psFocPMSM->bIqPiSatFlag, &psFocPMSM->sIqPiParams);

	/* Q current controller saturation flag */
	psFocPMSM->bIqPiSatFlag = psFocPMSM->sIqPiParams.bLimFlag;

	/* D, Q voltage application */
	psFocPMSM->sUDQReq.f16D = psFocPMSM->sUDQController.f16D;
	psFocPMSM->sUDQReq.f16Q = psFocPMSM->sUDQController.f16Q;	
#endif
	
   	/* 2-phase to 2-phase transformation to stationary ref. frame */
	GMCLIB_ParkInv_F16(&psFocPMSM->sUDQReq, &sAngle, &psFocPMSM->sUAlBeReq);

	/* Begin - voltage control */ 	
 	GMCLIB_ElimDcBusRipFOC_F16(psFocPMSM->f16UDcBusFilt, &psFocPMSM->sUAlBeReq, &psFocPMSM->sUAlBeComp);
 	
	psFocPMSM->uw16SectorSVM = GMCLIB_SvmStd_F16(&psFocPMSM->sUAlBeComp, &psFocPMSM->sDutyABC);
	/* End - voltage control */

	psFocPMSM->sAlignment.f32Position += psFocPMSM->sAlignment.f32Speed;
}

void MCSTRUC_PosCalc(MCSTRUC_POS_CAlC_ENCODER_T *ptr)
{	
	// --------------- deal with index counter ---------------------
	ptr->w16EncInc = sub(ptr->w16EncCnt, ptr->w16EncCnt_1);
	if(0 != ptr->w16EncInc)
	{
		if(ptr->w16EncCnt_1 > 0)
		{
			ptr->w16FlagDirection = 1;
		}
		else if(ptr->w16EncCnt_1 < 0)
		{
			ptr->w16FlagDirection = -1;
		}
	}
	ptr->w16ZCntInc = sub(ptr->w16ZCnt , ptr->w16ZCnt_1);
	if(ptr->w16ZCntInc != 0)
	{
		if(-1 == ptr->w16FlagDirection)
		{
			ptr->w16RealZCnt -= ptr->w16ZCntInc;  
		}
		else
		{
			ptr->w16RealZCnt += ptr->w16ZCntInc;
		}
	}
	ptr->w16EncCnt_1 = ptr->w16EncCnt;
	ptr->w16ZCnt_1 = ptr->w16ZCnt;
	// ---------------------------------------------------------------
	
	ptr->f32PosRelative = L_add( V3_L_mult_int(ptr->w16RealZCnt, ptr->f32CoeffZ), \
			V3_L_mult_int(sub(ptr->w16EncCnt, ptr->w16EncCntInit), ptr->f32CoeffEnc));
}

Frac32 MCSTRUC_PosControllerCalc(Frac32 f32Err, MCSTRUC_POS_CONTROLLER_T *ptr)
{
	ptr->f32ResultPreSat = L_mult_ls(V3_L_mult(f32Err, ptr->f32PropGain), ptr->f16BaseTransform);
	
	if(ptr->f32ResultPreSat > ptr->f32UpperLimit)
	{
		ptr->f32ResultAftSat = ptr->f32UpperLimit;
		ptr->w16SatFlag = 1;
	}
	else if(ptr->f32ResultPreSat < ptr->f32LowerLimit)
	{
		ptr->f32ResultAftSat = ptr->f32LowerLimit;
		ptr->w16SatFlag = -1;
	}
	else
	{
		ptr->f32ResultAftSat = ptr->f32ResultPreSat;
		ptr->w16SatFlag = 0;
	}
	ptr->f32SpeedRef = V3_L_mult_int(ptr->f32ResultAftSat, ((Word32)ptr->w32PropBase));
	return(ptr->f32SpeedRef);
}


void AngleCalculation(ANGLE_GENERATOR_T *ptr)
{
	ptr->f32Delta = L_mult_ls(ptr->f32DesiredFreq, ptr->f16Tmp)<<1; // Q1.31 = Q1.31 * Q1.15
	ptr->f32CurAngle += ptr->f32Delta;
}

/******************************************************************************
* Inline functions
******************************************************************************/


