/******************************************************************************
* 
* 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      M1_statemachine.c
*
* @author    B36524, B46639
* 
* @version   
* 
* @date      Feg-20-2014
* 
* @brief     Motor 1 state machine
*
*******************************************************************************
*
* Motor 1 state machine.
*
******************************************************************************/

/******************************************************************************
* Includes
******************************************************************************/
#include "PE_types.h"
#include "IO_MAP.h"
#include "M1_statemachine.h"
/******************************************************************************
* Constants
******************************************************************************/

/******************************************************************************
* Macros 
******************************************************************************/
#define		M1_POSITION_LOOP

#define		M1_ALIGN_METHOD		1   // 0= current vector; 1= hall detect; 2= search method

/******************************************************************************
* Types
******************************************************************************/
typedef enum {
    CALIB           = 0,
    READY           = 1,
    ALIGN          	= 2,
	SPIN			= 3,
	FREEWHEEL		= 4
} M1_RUN_SUBSTATE_T;         /* Run sub-states */

/******************************************************************************
* Global variables
******************************************************************************/
extern void M1_MC33937Config(void);
extern void M1_MC33937_Recovery(void);
UWord16 	uw16M1Flag33937Busy = 0;

/* M1 structure */
MCDEF_FOC_PMSM_ENC_SPEED_PI_T		gsM1_Drive;
MCDEF_CONTROL_LOOP_T				gsM1_StateRunLoop;

MCSTRUC_ADC_2_PHASE_ASSIGNMENT_T 	gsM1_IChannels;
GFLIB_VECTORLIMIT_T_F16 			gsM1_I;
GFLIB_VECTORLIMIT_T_F16 			gsM1_IOffset;

ANGLE_GENERATOR_T					gsM1_AngleGen;

bool                 				mbM1_SwitchAppOnOff;

/******************************************************************************
* Local variables
******************************************************************************/
static M1_RUN_SUBSTATE_T		msM1_StateRun;

/* M1 current phase channels */
/* =========================================================
 * IA - ANA0(0), IC - ANA1(1)
 * IB - ANB0(8), IC - ANB1(9) 
 * =========================================================*/
static		MCSTRUC_ADC_2_PHASE_ASSIGNMENT_T	msChannels[8] =
{		{M1_IC_ADC_0, M1_IB_ADC_1},\
		{M1_IC_ADC_0, M1_IB_ADC_1},\
		{M1_IA_ADC_0, M1_IC_ADC_1},\
		{M1_IA_ADC_0, M1_IC_ADC_1},\
		{M1_IA_ADC_0, M1_IB_ADC_1},\
		{M1_IA_ADC_0, M1_IB_ADC_1},\
		{M1_IC_ADC_0, M1_IB_ADC_1},\
		{M1_IC_ADC_0, M1_IB_ADC_1}
};

/*  hall signal  decoder  */
static		MCSTRUC_HALL_DECODER_T		msHall[7] = 
{
		{0,							0,		0},\
		{FRAC16(-180.0/180.0),		3,		5},\
		{FRAC16(-60.0/180.0),		6,		3},\
		{FRAC16(-120.0/180.0),		2,		1},\
		{FRAC16(60.0/180.0),		5,		6},\
		{FRAC16(120.0/180.0),		1,		4},\
		{FRAC16(0.0/180.0),			4,		2}
};

//Debug, b46639
static UWord16 uw16M1FlagFaultInit = 0; // waiting in fault state, observing for fault information

/******************************************************************************
* Local functions
******************************************************************************/

/*------------------------------------
 * User state machine functions
 * ----------------------------------*/
static void M1_StateFault(void);
static void M1_StateInit(void);
static void M1_StateStop(void);
static void M1_StateRun(void);

/*------------------------------------
 * User state-transition functions
 * ----------------------------------*/
static void M1_TransFaultInit(void);
static void M1_TransInitFault(void);
static void M1_TransInitStop(void);
static void M1_TransStopFault(void);
static void M1_TransStopRun(void);
static void M1_TransRunFault(void);
static void M1_TransRunStop(void);

/* State machine functions field (in pmem) */
__pmem static const SM_APP_STATE_FCN_T msSTATE = {M1_StateFault, M1_StateInit, M1_StateStop, M1_StateRun};


/* State-transition functions field (in pmem) */
__pmem static const SM_APP_TRANS_FCN_T msTRANS = {M1_TransFaultInit, M1_TransInitFault, M1_TransInitStop, M1_TransStopFault, M1_TransStopRun, M1_TransRunFault, M1_TransRunStop};

/* State machine structure declaration and initialization */
SM_APP_CTRL_T gsM1_Ctrl = 
{
	/* gsM1_Ctrl.psState, User state functions  */
	&msSTATE,
 	
 	/* gsM1_Ctrl.psTrans, User state-transition functions */
 	&msTRANS,
 
  	/* gsM1_Ctrl.uiCtrl, Default no control command */
  	SM_CTRL_NONE,
  	
  	/* gsM1_Ctrl.eState, Default state after reset */
  	INIT 	
};


/*------------------------------------
 * User sub-state machine functions
 * ----------------------------------*/
static void M1_StateRunCalib(void);
static void M1_StateRunReady(void);
static void M1_StateRunAlign(void);
static void M1_StateRunSpin(void);
static void M1_StateRunFreewheel(void);

static void M1_StateRunCalibSlow(void);
static void M1_StateRunReadySlow(void);
static void M1_StateRunAlignSlow(void);
static void M1_StateRunSpinSlow(void);
static void M1_StateRunFreewheelSlow(void);

/*------------------------------------
 * User sub-state-transition functions
 * ----------------------------------*/
static void M1_TransRunCalibReady(void);
static void M1_TransRunReadyAlign(void);
static void M1_TransRunAlignSpin(void);
static void M1_TransRunAlignReady(void);
static void M1_TransRunSpinFreewheel(void);
static void M1_TransRunFreewheelAlign(void);
static void M1_TransRunFreewheelReady(void);

/* Sub-state machine functions field (in pmem) */
__pmem static const PFCN_VOID_VOID mM1_STATE_RUN_TABLE[5][2] = 
{
		{M1_StateRunCalib, M1_StateRunCalibSlow},
		{M1_StateRunReady, M1_StateRunReadySlow},
		{M1_StateRunAlign, M1_StateRunAlignSlow},
		{M1_StateRunSpin, M1_StateRunSpinSlow},
		{M1_StateRunFreewheel, M1_StateRunFreewheelSlow}
};


static void M1_FaultDetection(void);
static void M1_GetCurrent(UWord16 uw16SVMSectorParam, GMCLIB_3COOR_T_F16* psIABCfbckParam, GFLIB_VECTORLIMIT_T_F16 *psIMeasured, MCSTRUC_ADC_OFFSET_T *psOffset);
static void M1_ADCChannelMapping(UWord16 uw16SVMSectorParam);

/******************************************************************************
* Global functions
******************************************************************************/

/***************************************************************************//*!
*
* @brief   FAULT state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateFault(void)
{
	/* Disables PWM outputs */
	M1_DISABLE_PWM_OUTPUT();

	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
	
    /* Disable user application switch */
    mbM1_SwitchAppOnOff = false;

	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
		
		M1_CLEAR_OVERCURRENT_FAULT();

		MC_FAULT_CLEAR(gsM1_Drive.sFaultIdPending, MC_FAULT_STARTUP_FAIL);
		MC_FAULT_CLEAR(gsM1_Drive.sFaultIdPending, MC_FAULT_LOAD_OVER);
		
		M1_FaultDetection();
		
        if (!MC_FAULT_ANY(gsM1_Drive.sFaultIdPending))
    	{
			if (gsM1_Drive.uw16CounterState == 0)
			{
				/* Clear fault command */
				if ((uw16M1FlagFaultInit == 1) && (0 == uw16M1Flag33937Busy))
				{
					uw16M1FlagFaultInit = 0;
					gsM1_Ctrl.uiCtrl |= SM_CTRL_FAULT_CLEAR;
				}
			}
			else
			{
				gsM1_Drive.uw16CounterState--;
			}
    	}
		else
		{
			gsM1_Drive.uw16CounterState = gsM1_Drive.uw16TimeFaultRelease;
		}
	}
}

/***************************************************************************//*!
*
* @brief   FAULT state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateInit(void)
{
	mbM1_SwitchAppOnOff	= false;

    MC_FAULT_CLEAR_ALL(gsM1_Drive.sFaultId);

    MC_FAULT_CLEAR_ALL(gsM1_Drive.sFaultIdPending);

	gsM1_Drive.sFaultThresholds.f16UDcBusOver 	= (Frac16)M1_OVERVOLT_LIMIT;
	gsM1_Drive.sFaultThresholds.f16UDcBusUnder 	= (Frac16)M1_UNDERVOLT_LIMIT;
//	gsM1_Drive.sFaultThresholds.f16SpeedOver 	= FRAC16(SPEED_MAX / SPEED_SCALE);
//	gsM1_Drive.sFaultThresholds.f16SpeedUnder 	= FRAC16(SPEED_MIN / SPEED_SCALE);

	/* ADC params */
	gsM1_Drive.sADCOffset.sPh0.u16Sh = 10;
	GDFLIB_FilterMAInit_F16(FRAC16(0.5), &gsM1_Drive.sADCOffset.sPh0);// set the accumulator to be 0.5
	
	gsM1_Drive.sADCOffset.sPh1.u16Sh = 10;
	GDFLIB_FilterMAInit_F16(FRAC16(0.5), &gsM1_Drive.sADCOffset.sPh1);
	
	gsM1_Drive.sADCOffset.sPh2.u16Sh = 10;
	GDFLIB_FilterMAInit_F16(FRAC16(0.5), &gsM1_Drive.sADCOffset.sPh2);
	
	gsM1_Drive.sADCOffset.sPh3.u16Sh = 10;
	GDFLIB_FilterMAInit_F16(FRAC16(0.5), &gsM1_Drive.sADCOffset.sPh3);
	
	gsM1_Drive.sADCOffset.sI.f16Ph0 = FRAC16(0.5);
	gsM1_Drive.sADCOffset.sI.f16Ph1 = FRAC16(0.5);
	gsM1_Drive.sADCOffset.sI.f16Ph2 = FRAC16(0.5);
	gsM1_Drive.sADCOffset.sI.f16Ph3 = FRAC16(0.5);
	
	/* PMSM FOC params */
    gsM1_Drive.sFocPMSM.sIdPiParams.a32PGain 		= M1_D_KP_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.a32IGain 		= M1_D_KI_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.f32IAccK_1 	    = 0;
    gsM1_Drive.sFocPMSM.sIdPiParams.f16UpperLim 	= M1_D_POS_LIMIT_ALIGN;  
    gsM1_Drive.sFocPMSM.sIdPiParams.f16LowerLim 	= M1_D_NEG_LIMIT_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.bLimFlag 		= 0;

    gsM1_Drive.sFocPMSM.sIqPiParams.a32PGain 		= M1_Q_KP_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIqPiParams.a32IGain 		= M1_Q_KI_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIqPiParams.f32IAccK_1 	    = 0;
    gsM1_Drive.sFocPMSM.sIqPiParams.f16UpperLim 	= M1_Q_POS_LIMIT_ALIGN;  
    gsM1_Drive.sFocPMSM.sIqPiParams.f16LowerLim 	= M1_Q_NEG_LIMIT_ALIGN;    
    gsM1_Drive.sFocPMSM.sIqPiParams.bLimFlag 		= 0;

    gsM1_Drive.sFocPMSM.sUDcBusFilter.sFltCoeff.f32B0    = FRAC32(M1_FILTER_UDCBUS_B1 / 2.0);
    gsM1_Drive.sFocPMSM.sUDcBusFilter.sFltCoeff.f32B1    = FRAC32(M1_FILTER_UDCBUS_B2 / 2.0);
    gsM1_Drive.sFocPMSM.sUDcBusFilter.sFltCoeff.f32A1    = FRAC32(M1_FILTER_UDCBUS_A2 / -2.0);
    GDFLIB_FilterIIR1Init_F16(&gsM1_Drive.sFocPMSM.sUDcBusFilter);

	gsM1_Drive.sFocPMSM.sAnglePosEl.f16Sin = 0;
	gsM1_Drive.sFocPMSM.sAnglePosEl.f16Cos = 0;
	gsM1_Drive.sFocPMSM.sAnglePosElUpdate.f16Sin = 0;
	gsM1_Drive.sFocPMSM.sAnglePosElUpdate.f16Cos = 0;

	gsM1_Drive.sFocPMSM.sIABC.f16A = 0;		// feedback 3ph currents
	gsM1_Drive.sFocPMSM.sIABC.f16B = 0;
	gsM1_Drive.sFocPMSM.sIABC.f16C = 0;

	gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0; // feedback 2ph currents
	gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;

	gsM1_Drive.sFocPMSM.sIDQ.f16D = 0;// feedback 2ph currents in rotation stationary
	gsM1_Drive.sFocPMSM.sIDQ.f16Q = 0;

	gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;

	gsM1_Drive.sFocPMSM.sIDQError.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQError.f16Q = 0;
	
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000; // 0.5
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;

	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;

	gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;

	gsM1_Drive.sFocPMSM.sUDQController.f16D = 0; //  the output of D-current controller
	gsM1_Drive.sFocPMSM.sUDQController.f16Q = 0; //  the output of Q-current controller

	gsM1_Drive.sFocPMSM.sAlignment.f32Position 			= 0;// f32Speed will be added to f32Position every current loop cycle
	gsM1_Drive.sFocPMSM.sAlignment.f32U 				= 0;	// initial D voltage length during alignment
	gsM1_Drive.sFocPMSM.sAlignment.f32Id				= 0;	// initial D current length during alignment
	gsM1_Drive.sFocPMSM.sAlignment.f16Id				= 0;
	gsM1_Drive.sFocPMSM.sAlignment.f32Speed 			= FRAC32(M1_ALIGN_SPEED / M1_ALIGN_SPEED_SCALE);
	gsM1_Drive.sFocPMSM.sAlignment.f32UStep 			= FRAC32(M1_ALIGN_VOLT_RAMP / M1_CONTROL_FREQ / M1_U_FOC_MAX);// step is added to f32U every current loop cycle
	gsM1_Drive.sFocPMSM.sAlignment.f32IdStep 			= FRAC32(M1_ALIGN_CURRENT_RAMP / M1_CONTROL_FREQ / M1_I_MAX);// step is added to f32Id every current loop cycle
	gsM1_Drive.sFocPMSM.sAlignment.f16IMax 				= FRAC16(M1_ALIGN_CURRENT / M1_I_MAX);// maximal D current threshold
	gsM1_Drive.sFocPMSM.sAlignment.f16UMax 				= FRAC16(M1_ALIGN_VOLT_MAX / M1_U_FOC_MAX);// maximal D voltage threshold
	gsM1_Drive.sFocPMSM.sAlignment.uw16TimeAlignment 	= (UWord16)(M1_DURATION_TASK_ALIGN * (M1_CONTROL_FREQ/M1_SPEED_LOOP_CNTR));

	gsM1_Drive.sFocPMSM.uw16SectorSVM = 2;
	gsM1_Drive.sFocPMSM.f16DutyCycleLimit = FRAC16(M1_DUTY_CYCLE_LIMIT);
	gsM1_Drive.sFocPMSM.f16UDcBus = 0;
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = 0;
    gsM1_Drive.sFocPMSM.bIdPiSatFlag = 0;
	gsM1_Drive.sFocPMSM.bIqPiSatFlag = 0;
	gsM1_Drive.sFocPMSM.bOpenLoop = false;
	gsM1_Drive.sFocPMSM.bUseMaxBus = false;
	gsM1_Drive.sFocPMSM.bUseZc = false;

	/* Speed params */
	// Speed controller
	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16BaseTransform 		= M1_SPD_CTRL_AW_BASE_TRANSFORM;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16Prop 				= M1_SPD_CTRL_AW_KP;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16Integ 				= M1_SPD_CTRL_AW_KI;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16Kc 					= M1_SPD_CTRL_AW_KC;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32UpperLimit 			= M1_SPD_CTRL_AW_OUTPUT_LIMIT;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32LowerLimit 			= -M1_SPD_CTRL_AW_OUTPUT_LIMIT;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.w16SatFlag 			= 0;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32IntegAcc			= FRAC32(0.0);  
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32SatErr 				= FRAC16(0.0);    
   	
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32LowSpeed 			= FRAC32(1.0L *M1_SPD_CTRL_AW_LOW_SPD /M1_N_MAX);
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32HighSpeed 			= FRAC32(1.0L *M1_SPD_CTRL_AW_HIGH_SPD /M1_N_MAX);
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32ReciprlSpeedScope 	= FRAC32(1.0L/(M1_SPD_CTRL_AW_HIGH_SPD - M1_SPD_CTRL_AW_LOW_SPD));
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16LowSpeedProp		= M1_SPD_CTRL_AW_LOW_SPD_KP;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16HighSpeedProp		= M1_SPD_CTRL_AW_HIGH_SPD_KP;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16PropScope			= sub(gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16LowSpeedProp, \
   																	  gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16HighSpeedProp);
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16LowSpeedInteg		= M1_SPD_CTRL_AW_LOW_SPD_KI;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16HighSpeedInteg		= M1_SPD_CTRL_AW_HIGH_SPD_KI;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16IntegScope 			= sub(gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16LowSpeedInteg,\
   																	  gsM1_Drive.sSpeedCtrl.sSpeedPIController.f16HighSpeedInteg);
   	
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.w32SpdBase				= M1_N_MAX;
   	gsM1_Drive.sSpeedCtrl.sSpeedPIController.w32KpBase				= M1_SPD_CTRL_AW_KP_BASE;
   	
   	// 32-bit IIR filter for the encoder speed
   	gsM1_Drive.sSpeedCtrl.sSpeedFilter.f32B1 = FRAC32(M1_FILTER_SPEED_B1 / (2.0)); 	// 100=0.1367287, 200=0.245237, 50=0.07295966,	30hz=0.045035006
   	gsM1_Drive.sSpeedCtrl.sSpeedFilter.f32B2 = FRAC32(M1_FILTER_SPEED_B2 / (2.0));	// B1 = B2
   	gsM1_Drive.sSpeedCtrl.sSpeedFilter.f32A2 = FRAC32(M1_FILTER_SPEED_A2 / (2.0));	// 100=-0.7265425, 200=-0.509525, 50=-0.8540807, 30hz=-0.909929988
   	IIR32FilterClear(&gsM1_Drive.sSpeedCtrl.sSpeedFilter);   	

   	// 32-bit ramp for the cmd speed, used in slow loop
   	gsM1_Drive.sSpeedCtrl.sSpeedRampParams.f32RampUp   	= M1_RAMP_INC_SPD_CL;
   	gsM1_Drive.sSpeedCtrl.sSpeedRampParams.f32RampDown 	= M1_RAMP_INC_SPD_CL;
   	gsM1_Drive.sSpeedCtrl.f32Speed 						= 0;
   	gsM1_Drive.sSpeedCtrl.f32SpeedCmd 					= 0;
   	gsM1_Drive.sSpeedCtrl.f32SpeedError 				= 0;
   	gsM1_Drive.sSpeedCtrl.f32SpeedFilt 					= 0;
   	gsM1_Drive.sSpeedCtrl.f32SpeedRamp 					= 0;
   	gsM1_Drive.sSpeedCtrl.f32SpeedReq 					= 0;
	gsM1_Drive.sSpeedCtrl.bOpenLoop 					= false;
	
	// Position and speed calculation 
	gsM1_Drive.sSpeedCalc.w16SpeedCalcScale				= M1_SPD_CALC_SCALE;
	gsM1_Drive.sSpeedCalc.w16SpeedCalcScaleShift		= M1_SPD_CALC_SCALE_SHIFT;
	gsM1_Drive.sSpeedCalc.f16PositionMechScale			= M1_ENC_SCALE;
	gsM1_Drive.sSpeedCalc.w16PositionMechScaleShift 	= M1_ENC_SHIFT_SCALE;
	gsM1_Drive.sSpeedCalc.f32Speed 						= 0;
//	gsM1_Drive.sSpeedCalc.w16SpeedCoeff					= M1_SPEED_ENC_CONST_H;

	gsM1_Drive.sSpeedPos.f16PositionMechScale		= M1_ENC_SCALE;
	gsM1_Drive.sSpeedPos.w16PositionMechScaleShift 	= M1_ENC_SHIFT_SCALE;
	gsM1_Drive.sSpeedPos.w16PolePairs 				= M1_POLE_PAIRS;
	gsM1_Drive.sSpeedPos.uw16PositionMechRawMask 	= 0xFFFF;
	gsM1_Drive.sSpeedPos.f32SpeedImproved 			= 0;
	gsM1_Drive.sSpeedPos.f32Speed 					= 0;
	gsM1_Drive.sSpeedPos.f16PositionEl 				= 0;
	gsM1_Drive.sSpeedPos.f16PositionElUpdate		= 0;
	gsM1_Drive.sSpeedPos.f16PositionMech 			= 0;
	
	// Position calculation for position loop
	gsM1_Drive.sPosCalc.f32CoeffZ					= M1_COEF_Z;
	gsM1_Drive.sPosCalc.f32CoeffEnc 				= M1_COEF_ENC;
	
	M1_CLEAR_Z_CNT();	// Clear Zcnt counter
	M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
	gsM1_Drive.sPosCalc.w16EncCntInit 				= gsM1_Drive.sPosCalc.w16EncCnt;
	gsM1_Drive.sPosCalc.w16EncCnt_1 				= gsM1_Drive.sPosCalc.w16EncCnt;
	gsM1_Drive.sPosCalc.w16ZCnt 					= 0;
	gsM1_Drive.sPosCalc.w16ZCnt_1 					= 0;
	gsM1_Drive.sPosCalc.w16RealZCnt 				= 0;
	gsM1_Drive.sPosCalc.w16FlagDirection 			= 0;
	gsM1_Drive.sPosCalc.f32PosRelative				= 0;
	
	/* position loop */
	gsM1_Drive.SPosCtrl.w16FlagPosFilter				= 0;
	gsM1_Drive.SPosCtrl.f32PosDesired					= FRAC32(0.0L/M1_POS_BASE); // 10 rounds
	gsM1_Drive.SPosCtrl.f32PosRamp						= 0;
	gsM1_Drive.SPosCtrl.PosRampInc.f32RampUp			= M1_POS_RAMP_UP; // 40rounds.S-1
	gsM1_Drive.SPosCtrl.PosRampInc.f32RampDown			= M1_POS_RAMP_DOWN;	
	gsM1_Drive.SPosCtrl.udtPosSmoothFilter.f32Gain 		= M1_POS_SMOOTHFILTER_GAIN;	// 50hz
	gsM1_Drive.SPosCtrl.w16FlagSineTest					= 0;
	gsM1_Drive.SPosCtrl.f32SineAmplitude				= M1_POS_SINE_WAVE_AMPLITUDE;
	SmoothFilterInit(&gsM1_Drive.SPosCtrl.udtPosSmoothFilter);
	
	/* position controller */
	gsM1_Drive.SPosCtrl.sPosController.w16SatFlag					= 0;
	gsM1_Drive.SPosCtrl.sPosController.f32PropGain					= M1_POS_CTRL_PROP_GAIN;
	gsM1_Drive.SPosCtrl.sPosController.f16BaseTransform 			= FRAC16(1.0 *M1_POS_BASE /M1_N_MAX);
	gsM1_Drive.SPosCtrl.sPosController.f32UpperLimit				= M1_POS_CTRL_UPPER_LIMIT;
	gsM1_Drive.SPosCtrl.sPosController.f32LowerLimit				= M1_POS_CTRL_LOWER_LIMIT;
	gsM1_Drive.SPosCtrl.sPosController.f32PropGainLow				= FRAC32(1.0L *M1_POS_PROP_GAIN_LOW /M1_POS_CTRL_PROP_GAIN_BASE);
	gsM1_Drive.SPosCtrl.sPosController.f32PropGainHigh				= FRAC32(1.0L *M1_POS_PROP_GAIN_HIGH /M1_POS_CTRL_PROP_GAIN_BASE);
	gsM1_Drive.SPosCtrl.sPosController.f32PropScope					= L_sub(gsM1_Drive.SPosCtrl.sPosController.f32PropGainHigh, gsM1_Drive.SPosCtrl.sPosController.f32PropGainLow);
	gsM1_Drive.SPosCtrl.sPosController.f32PosErrLow					= FRAC32(1.0L *M1_POS_ERR_LOW /M1_POS_BASE);
	gsM1_Drive.SPosCtrl.sPosController.f32PosErrHigh				= FRAC32(1.0L *M1_POS_ERR_HIGH /M1_POS_BASE);
	gsM1_Drive.SPosCtrl.sPosController.f32ReciprlPosErrScope		= FRAC32(1.0L/((M1_POS_ERR_HIGH - M1_POS_ERR_LOW) *M1_POS_ERR_AMPLIFICATION));
	gsM1_Drive.SPosCtrl.sPosController.w32PropBase					= M1_POS_CTRL_PROP_GAIN_BASE;
	
	/* Search rotor position */
	gsM1_Drive.sSearchRotorPos.w16FlagOutput 						= 0;
	gsM1_Drive.sSearchRotorPos.f16IdRamp							= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagState							= 0;
	gsM1_Drive.sSearchRotorPos.uw16FlagWrite						= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagMax							= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagSearchFinish 					= 0;
	gsM1_Drive.sSearchRotorPos.f16Angle_A 							= M1_SEARCH_ROTOR_ANGLE_A;
	gsM1_Drive.sSearchRotorPos.w32Angle_A							= (Word32)M1_SEARCH_ROTOR_ANGLE_A;
	gsM1_Drive.sSearchRotorPos.f16Angle_B 							= M1_SEARCH_ROTOR_ANGLE_B;
	gsM1_Drive.sSearchRotorPos.w32Angle_B							= (Word32)M1_SEARCH_ROTOR_ANGLE_B;
	gsM1_Drive.sSearchRotorPos.f16Angle_C 							= M1_SEARCH_ROTOR_ANGLE_C;
	gsM1_Drive.sSearchRotorPos.w32Angle_C							= (Word32)M1_SEARCH_ROTOR_ANGLE_C;
	gsM1_Drive.sSearchRotorPos.f16IdMax								= M1_SEARCH_ROTOR_ID_MAX;
	gsM1_Drive.sSearchRotorPos.f16IdMix								= M1_SEARCH_ROTOR_ID_MIX;
	gsM1_Drive.sSearchRotorPos.f16IdIncr							= M1_SEARCH_ROTOR_ID_INCR;
	gsM1_Drive.sSearchRotorPos.f16IdPreSet							= M1_SEARCH_ROTOR_ID_MIX;
	gsM1_Drive.sSearchRotorPos.w16ShockCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16ShockAmount						= M1_SEARCH_ROTOR_SHOCK_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16WaitCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16WaitAmount						= M1_SEARCH_ROTOR_WAIT_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16SpaceCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16SpaceAmount						= M1_SEARCH_ROTOR_SPACE_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16CounterPulseFilter				= 0;
	gsM1_Drive.sSearchRotorPos.w16AmountPulseFilter 				= M1_SEARCH_ROTOR_AMOUNT_PULSE_FILTER;
	gsM1_Drive.sSearchRotorPos.w16EncoderMixErr						= M1_SEARCH_ROTOR_ENC_MIX_ERR;
	gsM1_Drive.sSearchRotorPos.w16PolePairs							= M1_POLE_PAIRS;
	gsM1_Drive.sSearchRotorPos.f16AngleMixScope						= (Frac16)(mult_int(extract_h(L_mult(((Frac16)M1_SEARCH_ROTOR_ENC_MIX_SCOPE), M1_SEARCH_ROTOR_ENC_SCALE) << M1_SEARCH_ROTOR_ENC_SCALE_SHIFT), gsM1_Drive.sSearchRotorPos.w16PolePairs));
	gsM1_Drive.sSearchRotorPos.f16EncoderScale						= M1_SEARCH_ROTOR_ENC_SCALE;
	gsM1_Drive.sSearchRotorPos.w16EncoderScaleShift					= M1_SEARCH_ROTOR_ENC_SCALE_SHIFT;
	gsM1_Drive.sSearchRotorPos.f16EncoderModuleHalf					= M1_SEARCH_ROTOR_ENC_HALF;
	
	gsM1_Drive.sSearchRotorRampI.f16RampUp							= M1_SEARCH_ROTOR_I_RAMP;
	gsM1_Drive.sSearchRotorRampI.f16RampDown						= M1_SEARCH_ROTOR_I_RAMP;

	/* Angle Generator initialization for SineWave Test*/
	gsM1_AngleGen.f32StartAngle 		= 0;  		// simulated angle starts from 0 degree
	gsM1_AngleGen.w16BaseFreq			= M1_SINE_WAVE_FREQ_BASE;	// 200hz
	gsM1_AngleGen.w16ControlFreq		= M1_CONTROL_FREQ; 		// adc isr frequency
	gsM1_AngleGen.f32DesiredFreq		= FRAC32(1.0L /M1_SINE_WAVE_FREQ_BASE);
	gsM1_AngleGen.f32CurAngle 			= gsM1_AngleGen.f32StartAngle; // configure current angle
	gsM1_AngleGen.f16Tmp 				= div_ls((((Word32)(gsM1_AngleGen.w16BaseFreq))<<16), gsM1_AngleGen.w16ControlFreq); // Q1.15 = Q16.16/Q16.0
		
	/* Hall  */
	gsM1_Drive.sHall.f16IdDesired		= FRAC16(2000.0/M1_FM_I_SCALE);
	gsM1_Drive.sHall.f16IdRamp			= 0;
	gsM1_Drive.sHall.uw16State			= 0;
	gsM1_Drive.sHall.uw16AngleToEncoder = 0;
	
	gsM1_Drive.sHallRampI.f16RampUp		= FRAC16(0.5/M1_FM_I_SCALE);
	gsM1_Drive.sHallRampI.f16RampDown	= FRAC16(0.5/M1_FM_I_SCALE);
	
	// counters in the slow loops
	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
	}
	gsM1_Drive.uw16CounterSlowLoop 			= 1;
	gsM1_Drive.uw16DividerSlowLoop 			= M1_SPEED_LOOP_CNTR;
	gsM1_Drive.uw16CounterState 			= 0;
	gsM1_Drive.uw16TimeFullSpeedFreeWheel 	= (UWord16)(M1_DURATION_TASK_FREE_WHEEL * (M1_CONTROL_FREQ/M1_SPEED_LOOP_CNTR));
	gsM1_Drive.uw16TimeCalibration 			= (UWord16)(M1_DURATION_TASK_CALIB * (M1_CONTROL_FREQ/M1_SPEED_LOOP_CNTR));
	gsM1_Drive.uw16TimeFaultRelease 		= (UWord16)(M1_DURATION_TASK_FAULT_RELEASE * (M1_CONTROL_FREQ/M1_SPEED_LOOP_CNTR));

	/* Filter init not to enter to fault */
	gsM1_Drive.sFocPMSM.sUDcBusFilter.f16FltBfrX[0] = add((M1_OVERVOLT_LIMIT >> 1), (M1_UNDERVOLT_LIMIT >> 1)); 
	gsM1_Drive.sFocPMSM.sUDcBusFilter.f32FltBfrY[0] = L_deposit_h(add((M1_OVERVOLT_LIMIT >> 1), (M1_UNDERVOLT_LIMIT >> 1))); 

    gsM1_IChannels.uw16Ph0 = msChannels[2].uw16Ph0;
 	gsM1_IChannels.uw16Ph1 = msChannels[2].uw16Ph1;

	/* INIT_DONE command */
	gsM1_Ctrl.uiCtrl |= SM_CTRL_INIT_DONE;
}

/***************************************************************************//*!
*
* @brief   STOP state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateStop(void)
{
	/* Disable PWM output */
	M1_DISABLE_PWM_OUTPUT();

	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);

	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
		
		/* Speed filter */
		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);
		
		if (mbM1_SwitchAppOnOff)
		{
			/* Start command */
			gsM1_Ctrl.uiCtrl |= SM_CTRL_START;
		}
	}

	M1_FaultDetection();
	
	if (MC_FAULT_ANY(gsM1_Drive.sFaultId))
	{
		/* Fault state */
		gsM1_Ctrl.uiCtrl |= SM_CTRL_FAULT;
	}
}

/***************************************************************************//*!
*
* @brief   RUN state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRun(void)
{
	/* Run sub-state function */
	mM1_STATE_RUN_TABLE[msM1_StateRun][gsM1_StateRunLoop]();

	/* Detects faults */
	M1_FaultDetection();

	if (!mbM1_SwitchAppOnOff)
	{
		/* Stop command */
		gsM1_Ctrl.uiCtrl |= SM_CTRL_STOP;	
	}

	if (MC_FAULT_ANY(gsM1_Drive.sFaultId))
	{
		/* Fault state */
		gsM1_Ctrl.uiCtrl |= SM_CTRL_FAULT;
	}
}

/***************************************************************************//*!
*
* @brief   FAULT to INIT transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransFaultInit(void)
{    

}

/***************************************************************************//*!
*
* @brief   INIT to FAULT transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransInitFault(void)
{
	M1_DISABLE_PWM_OUTPUT();

	gsM1_Drive.uw16CounterState = gsM1_Drive.uw16TimeFaultRelease;	
}

/***************************************************************************//*!
*
* @brief   INIT to STOP transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransInitStop(void)
{

}

/***************************************************************************//*!
*
* @brief   STOP to FAULT transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransStopFault(void)
{
	/* Disables PWM outputs */
	M1_DISABLE_PWM_OUTPUT();
	
	gsM1_Drive.uw16CounterState = gsM1_Drive.uw16TimeFaultRelease;
}

/***************************************************************************//*!
*
* @brief   STOP to RUN transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransStopRun(void)
{
	gsM1_Drive.sSpeedCtrl.f32SpeedCmd = 1;
	gsM1_Drive.SPosCtrl.f32PosDesired = 0;
	
	gsM1_Drive.sFocPMSM.uw16SectorSVM = 2;
	
 	gsM1_IOffset.f16A = 0;
    gsM1_IOffset.f16B = 0;
    
 	gsM1_IChannels.uw16Ph0 = msChannels[gsM1_Drive.sFocPMSM.uw16SectorSVM].uw16Ph0;
 	gsM1_IChannels.uw16Ph1 = msChannels[gsM1_Drive.sFocPMSM.uw16SectorSVM].uw16Ph1;

	/* 50% duty cycle */
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;
	
	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
	
	/* Enable PWM output */
	M1_ENABLE_PWM_OUTPUT();

	/* Required time for ADC calibration */
	gsM1_Drive.uw16CounterState = gsM1_Drive.uw16TimeCalibration;

	/* Init sub-state when transition to RUN */
	msM1_StateRun = CALIB;

	/* Acknowledge that the system can proceed into the RUN state */
	gsM1_Ctrl.uiCtrl |= SM_CTRL_RUN_ACK;
	
	/* Search rotor position */
	gsM1_Drive.sSearchRotorPos.w16FlagOutput 						= 0;
	gsM1_Drive.sSearchRotorPos.f16IdRamp							= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagState							= 0;
	gsM1_Drive.sSearchRotorPos.uw16FlagWrite						= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagMax							= 0;
	gsM1_Drive.sSearchRotorPos.w16FlagSearchFinish 					= 0;
	gsM1_Drive.sSearchRotorPos.f16Angle_A 							= M1_SEARCH_ROTOR_ANGLE_A;
	gsM1_Drive.sSearchRotorPos.w32Angle_A							= (Word32)M1_SEARCH_ROTOR_ANGLE_A;
	gsM1_Drive.sSearchRotorPos.f16Angle_B 							= M1_SEARCH_ROTOR_ANGLE_B;
	gsM1_Drive.sSearchRotorPos.w32Angle_B							= (Word32)M1_SEARCH_ROTOR_ANGLE_B;
	gsM1_Drive.sSearchRotorPos.f16Angle_C 							= M1_SEARCH_ROTOR_ANGLE_C;
	gsM1_Drive.sSearchRotorPos.w32Angle_C							= (Word32)M1_SEARCH_ROTOR_ANGLE_C;
	gsM1_Drive.sSearchRotorPos.f16IdMax								= M1_SEARCH_ROTOR_ID_MAX;
	gsM1_Drive.sSearchRotorPos.f16IdMix								= M1_SEARCH_ROTOR_ID_MIX;
	gsM1_Drive.sSearchRotorPos.f16IdIncr							= M1_SEARCH_ROTOR_ID_INCR;
	gsM1_Drive.sSearchRotorPos.f16IdPreSet							= M1_SEARCH_ROTOR_ID_MIX;
	gsM1_Drive.sSearchRotorPos.w16ShockCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16ShockAmount						= M1_SEARCH_ROTOR_SHOCK_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16WaitCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16WaitAmount						= M1_SEARCH_ROTOR_WAIT_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16SpaceCnt							= 0;
	gsM1_Drive.sSearchRotorPos.w16SpaceAmount						= M1_SEARCH_ROTOR_SPACE_AMOUNT;
	gsM1_Drive.sSearchRotorPos.w16CounterPulseFilter				= 0;
	gsM1_Drive.sSearchRotorPos.w16AmountPulseFilter 				= M1_SEARCH_ROTOR_AMOUNT_PULSE_FILTER;
	gsM1_Drive.sSearchRotorPos.w16EncoderMixErr						= M1_SEARCH_ROTOR_ENC_MIX_ERR;
	gsM1_Drive.sSearchRotorPos.f16AngleMixScope						= (Frac16)(extract_h(L_mult(((Frac16)M1_SEARCH_ROTOR_ENC_MIX_SCOPE), M1_SEARCH_ROTOR_ENC_SCALE) << M1_SEARCH_ROTOR_ENC_SCALE_SHIFT));
	
	gsM1_Drive.sSearchRotorRampI.f16RampUp						= M1_SEARCH_ROTOR_I_RAMP;
	gsM1_Drive.sSearchRotorRampI.f16RampDown					= M1_SEARCH_ROTOR_I_RAMP;
	
	/* Hall  */
	gsM1_Drive.sHall.f16IdDesired		= FRAC16(2000.0/M1_FM_I_SCALE);
	gsM1_Drive.sHall.f16IdRamp			= 0;
	gsM1_Drive.sHall.uw16State			= 0;
	gsM1_Drive.sHall.uw16AngleToEncoder = 0;
}

/***************************************************************************//*!
*
* @brief   RUN to FAULT transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunFault(void)
{
	/* Disables PWM outputs */
	M1_DISABLE_PWM_OUTPUT();

	gsM1_Drive.sFocPMSM.sIABC.f16A = 0;
	gsM1_Drive.sFocPMSM.sIABC.f16B = 0;
	gsM1_Drive.sFocPMSM.sIABC.f16C = 0;
				
	gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;	

	gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;	
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;	

	gsM1_Drive.uw16CounterState = gsM1_Drive.uw16TimeFaultRelease;
}

/***************************************************************************//*!
*
* @brief   RUN to STOP transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunStop(void)
{
	gsM1_Drive.sSpeedCtrl.f32SpeedCmd = 0;
	
    switch(msM1_StateRun)
    {
        case (CALIB):
		{
			/* Acknowledge that the system can proceed into the STOP state */
			gsM1_Ctrl.uiCtrl |= SM_CTRL_STOP_ACK;			

			break;
		}
        case (READY):
        {
			/* Acknowledge that the system can proceed into the STOP state */
			gsM1_Ctrl.uiCtrl |= SM_CTRL_STOP_ACK;			

			break;
        }        
        case (ALIGN):
        {
			/* Disables PWM outputs */
			M1_DISABLE_PWM_OUTPUT();

			gsM1_Drive.sFocPMSM.sIABC.f16A = 0;
			gsM1_Drive.sFocPMSM.sIABC.f16B = 0;
			gsM1_Drive.sFocPMSM.sIABC.f16C = 0;

			gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
			gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
			gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
			gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;	

			gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0;
			gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;
			gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
			gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;	
			gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
			gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;	

			/* 50% duty cycle */
			gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
			gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
			gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

			/* PWM update */
			M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);

			/* Acknowledge that the system can proceed into the STOP state */
			gsM1_Ctrl.uiCtrl |= SM_CTRL_STOP_ACK;			

			break;
        }

        case (SPIN):
		{
			M1_TransRunSpinFreewheel();
			
			break;
		}

        case (FREEWHEEL):
		{
			if (gsM1_Drive.uw16CounterState == 0)
			{
				/* Disable PWM output */
				M1_DISABLE_PWM_OUTPUT();

				gsM1_Drive.sFocPMSM.sIABC.f16A = 0;
				gsM1_Drive.sFocPMSM.sIABC.f16B = 0;
				gsM1_Drive.sFocPMSM.sIABC.f16C = 0;
							
				gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
				gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
				gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
				gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;	

				gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0;
				gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;
				gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
				gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;	
				gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
				gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;	
				
				/* Acknowledge that the system can proceed into the STOP state */
				gsM1_Ctrl.uiCtrl |= SM_CTRL_STOP_ACK;			

				break;
			}
		}
    }
}

/***************************************************************************//*!
*
* @brief   RUN CALIB sub-state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunCalib(void)
{	
	/* Offset calculation */
	MCSTRUC_Current2ChannelUpdateOffset(gsM1_Drive.sFocPMSM.uw16SectorSVM, &gsM1_IOffset, &gsM1_Drive.sADCOffset);

	/* Alters the sector to get the other 2 channels */
	if (gsM1_Drive.sFocPMSM.uw16SectorSVM == 2)
	{
		gsM1_Drive.sFocPMSM.uw16SectorSVM = 1;
		
	}
	else
	{
		gsM1_Drive.sFocPMSM.uw16SectorSVM = 2;
	}
	
 	/* New channels to get the offset from */
	gsM1_IChannels.uw16Ph0 = msChannels[gsM1_Drive.sFocPMSM.uw16SectorSVM].uw16Ph0;
 	gsM1_IChannels.uw16Ph1 = msChannels[gsM1_Drive.sFocPMSM.uw16SectorSVM].uw16Ph1;
   
	/* 50% duty cycle */
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
}

/***************************************************************************//*!
*
* @brief   RUN CALIB sub-state for Slow loop
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunCalibSlow(void)
{
	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
	
	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
		
		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);
		
		if (--gsM1_Drive.uw16CounterState == 0)
		{
			/* Transition to the RUN READY sub-state */
			M1_TransRunCalibReady();	
		}
		
	}
}

/***************************************************************************//*!
*
* @brief   RUN READY sub-state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunReady(void)
{		
	/* 50% duty cycle */
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);

	/* Reads the 3-phase current */
	M1_GetCurrent(gsM1_Drive.sFocPMSM.uw16SectorSVM, &gsM1_Drive.sFocPMSM.sIABC, &gsM1_I, &gsM1_Drive.sADCOffset);
	
	gsM1_Drive.sFocPMSM.uw16SectorSVM = 2;

	/* ADC set up for the next step SVM sector */
	M1_ADCChannelMapping(gsM1_Drive.sFocPMSM.uw16SectorSVM);

	gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;
	gsM1_Drive.sFocPMSM.sIDQ.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQ.f16Q = 0;

	gsM1_Drive.sSpeedCtrl.f32SpeedReq = 0;
	gsM1_Drive.sSpeedCtrl.f32Speed = 0;

}

/***************************************************************************//*!
*
* @brief   RUN READY sub-state for Slow loop
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunReadySlow(void)
{
	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
			
	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
		
		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);

		if (!(gsM1_Drive.sSpeedCtrl.f32SpeedCmd == 0))
		{
			/* Transition to the RUN ALIGN sub-state */
			M1_TransRunReadyAlign();
		}
		
	}

}

/***************************************************************************//*!
*
* @brief   RUN ALIGN sub-state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunAlign(void)
{
	/* Reads the 3-phase current */
	M1_GetCurrent(gsM1_Drive.sFocPMSM.uw16SectorSVM, &gsM1_Drive.sFocPMSM.sIABC, &gsM1_I, &gsM1_Drive.sADCOffset);
		
#if	M1_ALIGN_METHOD == 0
	MCSTRUC_AlignmentPMSM(&gsM1_Drive.sFocPMSM);
#endif
	
#if	M1_ALIGN_METHOD == 1
	
	switch(gsM1_Drive.sHall.uw16State)
	{
	case 0:
		gsM1_Drive.sHall.f16PositionOrigin = msHall[gsM1_Drive.sHall.uw16Num].f16Angle;
		gsM1_Drive.sHall.f16PositionTarget = add(gsM1_Drive.sHall.f16PositionOrigin, FRAC16(90.0/180.0));
		gsM1_Drive.sHall.uw16Num_1 = gsM1_Drive.sHall.uw16Num;
		
		gsM1_Drive.sHall.uw16State = 1;
		break;
		
	case 1:
		if(gsM1_Drive.sHall.uw16Num == gsM1_Drive.sHall.uw16Num_1)
		{
			gsM1_Drive.sHall.f16IdRamp = GFLIB_Ramp_F16(gsM1_Drive.sHall.f16IdDesired, &gsM1_Drive.sHallRampI);
			
			/* Position for FOC */
			gsM1_Drive.sFocPMSM.sAnglePosEl.f16Sin = GFLIB_Sin_F16(gsM1_Drive.sHall.f16PositionTarget);
			gsM1_Drive.sFocPMSM.sAnglePosEl.f16Cos = GFLIB_Cos_F16(gsM1_Drive.sHall.f16PositionTarget);
			gsM1_Drive.sFocPMSM.sAnglePosElUpdate = gsM1_Drive.sFocPMSM.sAnglePosEl;
			/* set DQ current*/
			gsM1_Drive.sFocPMSM.sIDQReq.f16D = gsM1_Drive.sHall.f16IdRamp;
			gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;

			/* FOC */
			MCSTRUC_FocPMSMCurrentCtrl(&gsM1_Drive.sFocPMSM);
		}
		else
		{
			if(gsM1_Drive.sHall.uw16Num == msHall[gsM1_Drive.sHall.uw16Num_1].uw16CCW)
			{
				gsM1_Drive.sHall.uw16AngleToEncoder = (UWord16)mult(add(msHall[gsM1_Drive.sHall.uw16Num_1].f16Angle, FRAC16(30.0/180.0)), 1000);
			}
			else if(gsM1_Drive.sHall.uw16Num == msHall[gsM1_Drive.sHall.uw16Num_1].uw16CW)
			{
				gsM1_Drive.sHall.uw16AngleToEncoder = (UWord16)mult(sub(msHall[gsM1_Drive.sHall.uw16Num_1].f16Angle, FRAC16(30.0/180.0)), 1000);
			}
			M1_WRITE_ENC_CNT(gsM1_Drive.sHall.uw16AngleToEncoder);
			
			gsM1_Drive.sHall.uw16State = 2;
		}
		break;
		
	case 2:
		gsM1_Drive.sFocPMSM.sIDQ.f16D 					= 0;
		gsM1_Drive.sFocPMSM.sIDQ.f16Q					= 0;
		gsM1_Drive.sFocPMSM.sIDQReq.f16D				= 0;
		gsM1_Drive.sFocPMSM.sIDQReq.f16Q				= 0;
		gsM1_Drive.sFocPMSM.sIDQError.f16D				= 0;
		gsM1_Drive.sFocPMSM.sIDQError.f16Q				= 0;
		gsM1_Drive.sFocPMSM.sIdPiParams.f32IAccK_1		= 0;
		gsM1_Drive.sFocPMSM.sIqPiParams.f32IAccK_1		= 0;
		gsM1_Drive.sFocPMSM.sUDQReq.f16D				= 0;
		gsM1_Drive.sFocPMSM.sUDQReq.f16Q				= 0;
		
	 	/* 2-phase to 2-phase transformation to stationary ref. frame */
		GMCLIB_ParkInv_F16(&gsM1_Drive.sFocPMSM.sUDQReq, &gsM1_Drive.sFocPMSM.sAnglePosElUpdate, &gsM1_Drive.sFocPMSM.sUAlBeReq);
	    /* Begin - voltage control */
		GMCLIB_ElimDcBusRipFOC_F16(gsM1_Drive.sFocPMSM.f16UDcBusFilt, &gsM1_Drive.sFocPMSM.sUAlBeReq, &gsM1_Drive.sFocPMSM.sUAlBeComp);
		/* SVM */
	    gsM1_Drive.sFocPMSM.uw16SectorSVM = GMCLIB_SvmStd_F16(&gsM1_Drive.sFocPMSM.sUAlBeComp, &gsM1_Drive.sFocPMSM.sDutyABC);
		
	    gsM1_Drive.sFocPMSM.sDutyABC.f16A 				= FRAC16(0.5);
	    gsM1_Drive.sFocPMSM.sDutyABC.f16B 				= FRAC16(0.5);
	    gsM1_Drive.sFocPMSM.sDutyABC.f16C 				= FRAC16(0.5);
		break;
	}
	
#endif
	
#if	M1_ALIGN_METHOD == 2
	
	/* Search rotor position */
//	M1_SearchRotorPosCalc(&gsM1_Drive.sSearchRotorPos);
	
	if(0 == gsM1_Drive.sSearchRotorPos.w16FlagState)
	{
		M1_CLEAR_ENC_CNT();
	}
	
	M1_READ_ENC_CNT(gsM1_Drive.sSearchRotorPos.w16EncoderNow);
	SearchRotorPos(&gsM1_Drive.sSearchRotorPos);
	
	gsM1_Drive.sSearchRotorPos.f16IdRamp = GFLIB_Ramp16(gsM1_Drive.sSearchRotorPos.f16IdPreSet, gsM1_Drive.sSearchRotorPos.f16IdRamp, &gsM1_Drive.sSearchRotorRampI);
	
	if(1 == gsM1_Drive.sSearchRotorPos.uw16FlagWrite)
	{
		M1_WRITE_ENC_CNT(gsM1_Drive.sSearchRotorPos.uw16EncoderOutput);
		gsM1_Drive.sSearchRotorPos.uw16FlagWrite = 0;
	}
	
	/* Position for FOC */
	gsM1_Drive.sFocPMSM.sAnglePosEl.f16Sin = GFLIB_Sin_F16(gsM1_Drive.sSearchRotorPos.f16AngleOutput);
	gsM1_Drive.sFocPMSM.sAnglePosEl.f16Cos = GFLIB_Cos_F16(gsM1_Drive.sSearchRotorPos.f16AngleOutput);
	gsM1_Drive.sFocPMSM.sAnglePosElUpdate = gsM1_Drive.sFocPMSM.sAnglePosEl;
	/* set DQ current*/
	gsM1_Drive.sFocPMSM.sIDQReq.f16D = gsM1_Drive.sSearchRotorPos.f16IdRamp;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
	
	if(gsM1_Drive.sSearchRotorPos.w16FlagOutput == ENABLE)
	{
		/* FOC */
		MCSTRUC_FocPMSMCurrentCtrl(&gsM1_Drive.sFocPMSM);
	}
	else
	{
		gsM1_Drive.sFocPMSM.sIDQ.f16D 					= 0;
		gsM1_Drive.sFocPMSM.sIDQ.f16Q					= 0;
		gsM1_Drive.sFocPMSM.sIDQReq.f16D				= 0;
		gsM1_Drive.sFocPMSM.sIDQReq.f16Q				= 0;
		gsM1_Drive.sFocPMSM.sIDQError.f16D				= 0;
		gsM1_Drive.sFocPMSM.sIDQError.f16Q				= 0;
		gsM1_Drive.sFocPMSM.sIdPiParams.f32IntegPartK_1	= 0;
		gsM1_Drive.sFocPMSM.sIqPiParams.f32IntegPartK_1	= 0;
		gsM1_Drive.sFocPMSM.sUDQReq.f16D				= 0;
		gsM1_Drive.sFocPMSM.sUDQReq.f16Q				= 0;
		
	 	/* 2-phase to 2-phase transformation to stationary ref. frame */
	 	MCLIB_ParkTrfInv(&gsM1_Drive.sFocPMSM.sUAlBeReq, &gsM1_Drive.sFocPMSM.sUDQReq, &gsM1_Drive.sFocPMSM.sAnglePosElUpdate);
	    /* Begin - voltage control */
	    MCLIB_ElimDcBusRipGen(gsM1_Drive.sFocPMSM.f16UDcBusFilt, &gsM1_Drive.sFocPMSM.sUAlBeReq, &gsM1_Drive.sFocPMSM.sUAlBeComp);
		/* SVM */
	    gsM1_Drive.sFocPMSM.uw16SectorSVM = MCLIB_SvmStd(&gsM1_Drive.sFocPMSM.sUAlBeComp, &gsM1_Drive.sFocPMSM.sDutyABC);
		
	    gsM1_Drive.sFocPMSM.sDutyABC.f16A 				= FRAC16(0.5);
	    gsM1_Drive.sFocPMSM.sDutyABC.f16B 				= FRAC16(0.5);
	    gsM1_Drive.sFocPMSM.sDutyABC.f16C 				= FRAC16(0.5);
	}
#endif

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
	
	/* ADC set up for the next step SVM sector */
	M1_ADCChannelMapping(gsM1_Drive.sFocPMSM.uw16SectorSVM);// update gsM1_IChannels
}

/***************************************************************************//*!
*
* @brief   RUN ALIGN sub-state for Slow loop
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunAlignSlow(void)
{
	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
		
	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;

		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);
		
#if	M1_ALIGN_METHOD == 2
		if (1 == gsM1_Drive.sSearchRotorPos.w16FlagSearchFinish)
		{
			if(--gsM1_Drive.uw16CounterState == 0)
			{
				/* Transition to the RUN SPIN sub-state */
				M1_TransRunAlignSpin();
			}
		}
#endif
			
#if M1_ALIGN_METHOD == 0
		if (1 == gsM1_Drive.sSearchRotorPos.w16FlagSearchFinish)
		{
			if(--gsM1_Drive.uw16CounterState == 0)
			{
				M1_CLEAR_ENC_CNT(); // pull strongly to A axis, clear encoder counter
				/* Transition to the RUN SPIN sub-state */
				M1_TransRunAlignSpin();
			}
		}
#endif
			
#if M1_ALIGN_METHOD == 1
		if(2 == gsM1_Drive.sHall.uw16State)
		{
			if(--gsM1_Drive.uw16CounterState == 0)
			{
				M1_TransRunAlignSpin();
			}
		}
#endif

		if (gsM1_Drive.sSpeedCtrl.f32SpeedCmd == 0)
		{
			/* Transition to the RUN READY sub-state */
			M1_TransRunAlignReady();
		}
	}

	
}

/***************************************************************************//*!
*
* @brief   RUN SPIN sub-state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunSpin(void)
{
	/* Reads the 3-phase current */
	M1_GetCurrent(gsM1_Drive.sFocPMSM.uw16SectorSVM, &gsM1_Drive.sFocPMSM.sIABC, &gsM1_I, &gsM1_Drive.sADCOffset);

	/* Position for FOC */
	gsM1_Drive.sFocPMSM.sAnglePosEl = gsM1_Drive.sSpeedPos.sAnglePosEl;
	gsM1_Drive.sFocPMSM.sAnglePosElUpdate = gsM1_Drive.sSpeedPos.sAnglePosElUpdate;
		
	/* FOC */
	MCSTRUC_FocPMSMCurrentCtrl(&gsM1_Drive.sFocPMSM);

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
	
	/* ADC set up for the next step SVM sector */
	M1_ADCChannelMapping(gsM1_Drive.sFocPMSM.uw16SectorSVM);
	
	/* generate sine signal when sine wave testing in position loop */
	if (1 == gsM1_Drive.SPosCtrl.w16FlagSineTest)
	{
		AngleCalculation(&gsM1_AngleGen);
	}
}

/***************************************************************************//*!
*
* @brief   RUN SPIN sub-state for Slow loop
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunSpinSlow(void)
{
	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
	
	/* Speed loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;

		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);

#ifdef	M1_POSITION_LOOP
		
		/* generate sine signal when sine wave testing in position loop */
		if (1 == gsM1_Drive.SPosCtrl.w16FlagSineTest)
		{
			gsM1_Drive.SPosCtrl.f32PosDesired = L_mult_ls(gsM1_Drive.SPosCtrl.f32SineAmplitude, GFLIB_Sin_F16(extract_h(gsM1_AngleGen.f32CurAngle)));
		}
		/* choice one kind of filter group types and calculate position error */
		switch(gsM1_Drive.SPosCtrl.w16FlagPosFilter)
		{
		case 0:
		default:
			/* f32PosDesired --> f32PosRamp --> f32PosSmooth --> f32PError */
			gsM1_Drive.SPosCtrl.f32PosRamp = GFLIB_Ramp_F32(gsM1_Drive.SPosCtrl.f32PosDesired, &gsM1_Drive.SPosCtrl.PosRampInc);
			gsM1_Drive.SPosCtrl.f32PosSmooth = SmoothFilterCalc(gsM1_Drive.SPosCtrl.f32PosRamp, &gsM1_Drive.SPosCtrl.udtPosSmoothFilter);
			gsM1_Drive.SPosCtrl.f32PError =  L_sub(gsM1_Drive.SPosCtrl.f32PosSmooth, gsM1_Drive.sPosCalc.f32PosRelative);
			break;
			
		case 1:
			/* f32PosDesired --> f32PosRamp --> f32PError */
			gsM1_Drive.SPosCtrl.f32PosRamp = GFLIB_Ramp_F32(gsM1_Drive.SPosCtrl.f32PosDesired, &gsM1_Drive.SPosCtrl.PosRampInc);
			gsM1_Drive.SPosCtrl.f32PError =  L_sub(gsM1_Drive.SPosCtrl.f32PosRamp, gsM1_Drive.sPosCalc.f32PosRelative);
			break;
			
		case 2:
			/* f32PosDesired --> f32PosSmooth --> f32PError */
			gsM1_Drive.SPosCtrl.f32PosSmooth = SmoothFilterCalc(gsM1_Drive.SPosCtrl.f32PosDesired, &gsM1_Drive.SPosCtrl.udtPosSmoothFilter);
			gsM1_Drive.SPosCtrl.f32PError =  L_sub(gsM1_Drive.SPosCtrl.f32PosSmooth, gsM1_Drive.sPosCalc.f32PosRelative);
			break;
			
		case 3:
			/* f32PosDesired --> f32PError */
			gsM1_Drive.SPosCtrl.f32PError =  L_sub(gsM1_Drive.SPosCtrl.f32PosDesired, gsM1_Drive.sPosCalc.f32PosRelative);
			break;
		}
		
		/* position controller */
		gsM1_Drive.sSpeedCtrl.f32SpeedRamp = MCSTRUC_PosControllerCalc(gsM1_Drive.SPosCtrl.f32PError, &gsM1_Drive.SPosCtrl.sPosController);
//		gsM1_Drive.sSpeedCtrl.f32SpeedReq = MCSTRUC_PosControllerCalc(gsM1_Drive.SPosCtrl.f32PError, &gsM1_Drive.SPosCtrl.sPosController);
#endif
		
#ifndef	M1_POSITION_LOOP
		/* Speed ramp generation */
		gsM1_Drive.sSpeedCtrl.f32SpeedRamp = GFLIB_Ramp_F32(gsM1_Drive.sSpeedCtrl.f32SpeedCmd, &gsM1_Drive.sSpeedCtrl.sSpeedRampParams);
#endif
		/* Speed error calculation */
		gsM1_Drive.sSpeedCtrl.f32SpeedError = L_sub(gsM1_Drive.sSpeedCtrl.f32SpeedRamp, gsM1_Drive.sSpeedCtrl.f32SpeedFilt);

		if (!gsM1_Drive.sSpeedCtrl.bOpenLoop)
		{
			/* Desired current by the speed PI controller */
			gsM1_Drive.sFocPMSM.sIDQReq.f16Q = MCSTRUC_SpdCtrlAntiWindupCalc(&gsM1_Drive.sSpeedCtrl.sSpeedPIController, gsM1_Drive.sSpeedCtrl.f32SpeedError);
		}
		
		if (gsM1_Drive.sSpeedCtrl.f32SpeedCmd == 0)
		{
			/* Sub-state RUN FREEWHEEL */
			M1_TransRunSpinFreewheel();
		}
	}
	
}

/***************************************************************************//*!
*
* @brief   RUN FREEWHEEL sub-state
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunFreewheel(void)
{
	/* Disable PWM output */
	M1_DISABLE_PWM_OUTPUT();

	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);

	/* Reads the 3-phase current */
	M1_GetCurrent(gsM1_Drive.sFocPMSM.uw16SectorSVM, &gsM1_Drive.sFocPMSM.sIABC, &gsM1_I, &gsM1_Drive.sADCOffset);	

	gsM1_Drive.sFocPMSM.uw16SectorSVM = 2;

	/* ADC set up for the next step SVM sector */
	M1_ADCChannelMapping(gsM1_Drive.sFocPMSM.uw16SectorSVM);

}

/***************************************************************************//*!
*
* @brief   RUN FREEWHEEL sub-state for Slow loop
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_StateRunFreewheelSlow(void)
{
	/* DC bus voltage filter */
	gsM1_Drive.sFocPMSM.f16UDcBusFilt = GDFLIB_FilterIIR1_F16(gsM1_Drive.sFocPMSM.f16UDcBus, &gsM1_Drive.sFocPMSM.sUDcBusFilter);
	
	/* Slow loop control */
    if(gsM1_Drive.uw16FlagSlowLoop)
	{
        gsM1_Drive.uw16FlagSlowLoop = 0;
		
		gsM1_Drive.sSpeedCtrl.f32SpeedFilt = IIR32FilterCalc(&gsM1_Drive.sSpeedCtrl.sSpeedFilter, gsM1_Drive.sSpeedPos.f32SpeedImproved);
		
		/* Calculate the position for position loop */
		M1_READ_Z_CNT(gsM1_Drive.sPosCalc.w16ZCnt);
		M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
		MCSTRUC_PosCalc(&gsM1_Drive.sPosCalc);

		if (--gsM1_Drive.uw16CounterState == 0)
		{
			/* If app switch is on */
			if (mbM1_SwitchAppOnOff)
			{
//				gsM1_Drive.sSpeedCtrl.f32Speed = 0;
				gsM1_Drive.sSpeedCtrl.f32SpeedFilt = 0;
				
				/* If speed command is non-zero */
				if (abs_s(gsM1_Drive.sSpeedCtrl.f32SpeedCmd > 0))
				{
					/* If the motor has been in the run phase */
						/* Sub-state RUN READY */
						M1_TransRunFreewheelReady();
				}
				/* If speed command is zero */
				else
				{
					/* Sub-state RUN READY */
					M1_TransRunFreewheelReady();	
				}
			}
		}
	}

}

/***************************************************************************//*!
*
* @brief   RUN CALIB to READY transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunCalibReady(void)
{
	/* Sub-state RUN READY */
	msM1_StateRun = READY;
}

/***************************************************************************//*!
*
* @brief   RUN READY to ALIGN transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunReadyAlign(void)
{
	gsM1_Drive.sFocPMSM.sAlignment.f32Speed = L_abs(gsM1_Drive.sFocPMSM.sAlignment.f32Speed);
	
	if (gsM1_Drive.sSpeedCtrl.f32SpeedCmd < 0)
	{
		gsM1_Drive.sFocPMSM.sAlignment.f32Speed = L_negate(gsM1_Drive.sFocPMSM.sAlignment.f32Speed);
	}
	
	/* Alignment voltage init point */
	gsM1_Drive.sFocPMSM.sAlignment.f32U = 0;
	
	/* Alignment current init point */
	gsM1_Drive.sFocPMSM.sAlignment.f32Id = 0;
	gsM1_Drive.sFocPMSM.sAlignment.f16Id = 0;
	
	/* Alignment voltage init position */
	gsM1_Drive.sFocPMSM.sAlignment.f32Position = 0;
	
	/* Alignment duration set-up */
	gsM1_Drive.uw16CounterState = gsM1_Drive.sFocPMSM.sAlignment.uw16TimeAlignment;
	
	/* Sub-state RUN ALIGN */
	msM1_StateRun = ALIGN;
}

/***************************************************************************//*!
*
* @brief   RUN ALIGN to SPIN transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunAlignSpin(void)
{
	/* PMSM FOC params */
    gsM1_Drive.sFocPMSM.sIdPiParams.a32PGain 		= M1_D_KP_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.a32IGain 		= M1_D_KI_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.f32IAccK_1 	    = 0;
    gsM1_Drive.sFocPMSM.sIdPiParams.f16UpperLim 	= M1_D_POS_LIMIT_ALIGN;  
    gsM1_Drive.sFocPMSM.sIdPiParams.f16LowerLim 	= M1_D_NEG_LIMIT_ALIGN;
    gsM1_Drive.sFocPMSM.sIdPiParams.bLimFlag 		= 0;

    gsM1_Drive.sFocPMSM.sIqPiParams.a32PGain 		= M1_Q_KP_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIqPiParams.a32IGain 		= M1_Q_KI_GAIN_A32_ALIGN;
    gsM1_Drive.sFocPMSM.sIqPiParams.f32IAccK_1 	    = 0;
    gsM1_Drive.sFocPMSM.sIqPiParams.f16UpperLim 	= M1_Q_POS_LIMIT_ALIGN;  
    gsM1_Drive.sFocPMSM.sIqPiParams.f16LowerLim 	= M1_Q_NEG_LIMIT_ALIGN;    
    gsM1_Drive.sFocPMSM.sIqPiParams.bLimFlag 		= 0;

    gsM1_Drive.sFocPMSM.bIdPiSatFlag 					= 0;
	gsM1_Drive.sFocPMSM.bIqPiSatFlag 					= 0;
	
	gsM1_Drive.sFocPMSM.sIDQReq.f16D 					= 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q 					= 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16D 					= 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q 					= 0;	

	gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha 				= 0;
	gsM1_Drive.sFocPMSM.sIAlBe.f16Beta 					= 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha 				= 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta 				= 0;	
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha 			= 0;
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta 				= 0;	

	/* Speed params */
	// Speed controller
	gsM1_Drive.sSpeedCtrl.f32SpeedRamp 						= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.f32SpeedReq 						= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.f32SpeedError 					= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.f32Speed 							= FRAC32(0.0);
	
	IIR32FilterClear(&gsM1_Drive.sSpeedCtrl.sSpeedFilter); 
	gsM1_Drive.sSpeedCtrl.f32SpeedFilt 						= FRAC32(0.0);

	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32IntegAcc 	= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32PropAcc		= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32KcPart 		= FRAC32(0.0);
	gsM1_Drive.sSpeedCtrl.sSpeedPIController.f32SatErr		= FRAC32(0.0);
	
	gsM1_Drive.sSpeedCtrl.sSpeedPIController.w16SatFlag 	= 0;
	
	/* Position loop */
	SmoothFilterInit(&gsM1_Drive.SPosCtrl.udtPosSmoothFilter);
	gsM1_Drive.SPosCtrl.f32PosRamp						= 0;
	gsM1_Drive.SPosCtrl.f32PosSmooth 					= 0;
	gsM1_Drive.SPosCtrl.f32PError						= 0;
	
	// Position calculation for position loop	
	M1_CLEAR_Z_CNT();	// Clear Zcnt counter
	M1_READ_ENC_CNT(gsM1_Drive.sPosCalc.w16EncCnt);
	gsM1_Drive.sPosCalc.w16EncCntInit 				= gsM1_Drive.sPosCalc.w16EncCnt;
	gsM1_Drive.sPosCalc.w16EncCnt_1 				= gsM1_Drive.sPosCalc.w16EncCnt;
	gsM1_Drive.sPosCalc.w16ZCnt 					= 0;
	gsM1_Drive.sPosCalc.w16ZCnt_1 					= 0;
	gsM1_Drive.sPosCalc.w16RealZCnt 				= 0;
	gsM1_Drive.sPosCalc.w16FlagDirection 			= 0;	

	/* 50% duty cycle */
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
	
	/* limit the PI controller of Id and Iq */
	gsM1_Drive.sFocPMSM.bUseMaxBus = true;
	
	gsM1_Drive.uw16CounterSlowLoop = 1;
	
	/* Sub-state RUN SPIN */
	msM1_StateRun = SPIN;
}

/***************************************************************************//*!
*
* @brief   RUN ALIGN to READY transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunAlignReady(void)
{
	gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;	

	gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;	
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;	

	/* 50% duty cycle */
	gsM1_Drive.sFocPMSM.sDutyABC.f16A = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16B = 0x4000;
	gsM1_Drive.sFocPMSM.sDutyABC.f16C = 0x4000;

	/* PWM update */
	M1_PWM_UPDATE(&gsM1_Drive.sFocPMSM.sDutyABC);
	
	/* Sub-state RUN READY */
	msM1_StateRun = READY;
}

/***************************************************************************//*!
*
* @brief   RUN SPIN to FREEWHEEL transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunSpinFreewheel(void)
{
	/* Generates a time gap before the alignment to assure the rotor is not rotating */
	gsM1_Drive.uw16CounterState = (UWord16)mult_r(abs_s(extract_h(gsM1_Drive.sSpeedCtrl.f32SpeedFilt)), (Frac16)gsM1_Drive.uw16TimeFullSpeedFreeWheel);				

	gsM1_Drive.sFocPMSM.sIDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sIDQReq.f16Q = 0;

	gsM1_Drive.sFocPMSM.sUDQReq.f16D = 0;
	gsM1_Drive.sFocPMSM.sUDQReq.f16Q = 0;	

	gsM1_Drive.sFocPMSM.sIAlBe.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sIAlBe.f16Beta = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeReq.f16Beta = 0;	
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Alpha = 0;
	gsM1_Drive.sFocPMSM.sUAlBeComp.f16Beta = 0;	
	
	/* Sub-state RUN FREEWHEEL */
	msM1_StateRun = FREEWHEEL;	
}

/***************************************************************************//*!
*
* @brief   RUN FREEWHEEL to ALIGN transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunFreewheelAlign(void)
{
	/* Enable PWM output */
	M1_ENABLE_PWM_OUTPUT();

	/* Alignment voltage init point */
	gsM1_Drive.sFocPMSM.sAlignment.f32U = 0;

	/* Alignment voltage init position */
	gsM1_Drive.sFocPMSM.sAlignment.f32Position = 0;

	/* Alignment duration set-up */
	gsM1_Drive.uw16CounterState = gsM1_Drive.sFocPMSM.sAlignment.uw16TimeAlignment;

	/* Sub-state RUN ALIGN */
	msM1_StateRun = ALIGN;
}

/***************************************************************************//*!
*
* @brief   RUN FREEWHEEL to READY transition
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_TransRunFreewheelReady(void)
{
	/* Enable PWM output */
	M1_ENABLE_PWM_OUTPUT();

	/* Sub-state RUN READY */
	msM1_StateRun = READY;
}

/***************************************************************************//*!
*
* @brief   Fault Detection function
*
* @param   void
*
* @return  none
*
******************************************************************************/
static void M1_FaultDetection(void)
{
    //-----------------------------
    // Actual Faults
    //-----------------------------

    // Fault:   DC-bus over-voltage
	if (gsM1_Drive.sFocPMSM.f16UDcBusFilt >= gsM1_Drive.sFaultThresholds.f16UDcBusOver)
	{
		MC_FAULT_SET(gsM1_Drive.sFaultIdPending, MC_FAULT_U_DCBUS_OVER);
	}
	else
	{
		MC_FAULT_CLEAR(gsM1_Drive.sFaultIdPending, MC_FAULT_U_DCBUS_OVER);
	}

    // Fault:   DC-bus over-current
    if (M1_OVERCURRENT_FAULT())
    {
		MC_FAULT_SET(gsM1_Drive.sFaultIdPending, MC_FAULT_I_DCBUS_OVER);    	
    }
    else
    {
		MC_FAULT_CLEAR(gsM1_Drive.sFaultIdPending, MC_FAULT_I_DCBUS_OVER);
    }
    
    // Fault:   DC-bus under-voltage
    if (gsM1_Drive.sFocPMSM.f16UDcBusFilt <= gsM1_Drive.sFaultThresholds.f16UDcBusUnder)
    {
		MC_FAULT_SET(gsM1_Drive.sFaultIdPending, MC_FAULT_U_DCBUS_UNDER);
    }
    else
    {
		MC_FAULT_CLEAR(gsM1_Drive.sFaultIdPending, MC_FAULT_U_DCBUS_UNDER);
    }	
    
    gsM1_Drive.sFaultId |= gsM1_Drive.sFaultIdPending;     
}

/******************************************************************************
*
* 		   "gsM1_IChannels.uw16Ph0/1" are updated. ADC CLIST registers will be 
* 		   updated by the value in "gsM1_IChannels.uw16Ph0/1" when SVM sector is 
* 		   updated.
*
******************************************************************************/
static void M1_ADCChannelMapping(UWord16 uw16SVMSectorParam)
{
 	gsM1_IChannels.uw16Ph0 = msChannels[uw16SVMSectorParam].uw16Ph0;
 	gsM1_IChannels.uw16Ph1 = msChannels[uw16SVMSectorParam].uw16Ph1;
}

/***************************************************************************//*!
*
* @brief   Measure phase current
*
* @param   void
*	
* @return  none
*
******************************************************************************
*
* 		   In every sector, two motor phase currents sensed, the third calculated
*
******************************************************************************/
static void M1_GetCurrent(UWord16 uw16SVMSectorParam, GMCLIB_3COOR_T_F16* psIABCfbckParam, GFLIB_VECTORLIMIT_T_F16 *psIMeasured, MCSTRUC_ADC_OFFSET_T *psOffset)
{ 	
 	/*
 	 * 	ph0: ANA_I_A
 	 * 	ph1: ANB_I_B
 	 * 	ph2: ANA_I_C
 	 * 	PH3: ANB_I_C
 	 * */
	
	//*************** 3-PHASE CURRENT RECONSTRUCTION BEGIN **************
    switch (uw16SVMSectorParam) 
    {
    case 2:
    case 3: 
    		{	//direct sensing of A, C, calculation of B 
				psIABCfbckParam->f16A = (sub(psOffset->sI.f16Ph0, psIMeasured->f16A) << 1);
				psIABCfbckParam->f16C = (sub(psOffset->sI.f16Ph3, psIMeasured->f16B) << 1);
				psIABCfbckParam->f16B = negate(add(psIABCfbckParam->f16A, psIABCfbckParam->f16C));
			return;
	       	}
	       	break;
	       
    case 4: 
    case 5: 
    		{	//direct sensing of A, B, calculation of C 
				psIABCfbckParam->f16A = (sub(psOffset->sI.f16Ph0, psIMeasured->f16A) << 1);
				psIABCfbckParam->f16B = (sub(psOffset->sI.f16Ph1, psIMeasured->f16B) << 1);
                psIABCfbckParam->f16C = negate(add(psIABCfbckParam->f16A, psIABCfbckParam->f16B));
			return;
	        }		        
            break;
    case 1:
    case 6:
    default:
	        {	//direct sensing of C, B, calculation of A  
                psIABCfbckParam->f16C = (sub(psOffset->sI.f16Ph2, psIMeasured->f16A) << 1);
                psIABCfbckParam->f16B = (sub(psOffset->sI.f16Ph1, psIMeasured->f16B) << 1);
	    		psIABCfbckParam->f16A = negate(add(psIABCfbckParam->f16B, psIABCfbckParam->f16C));
            return;
	        }
    break;
    }
	//*************** 3-PHASE CURRENT RECONSTRUCTION -END- **************

}



/***************************************************************************//*!
*
* @brief   Set the app switch function
*
* @param   void
*
* @return  none
*
******************************************************************************/
void M1_SetAppSwitch(bool bValue)
{
	mbM1_SwitchAppOnOff = bValue;
}

/***************************************************************************//*!
*
* @brief   Read the app switch function
*
* @param   void
*
* @return  none
*
******************************************************************************/
bool M1_GetAppSwitch(void)
{
	return (mbM1_SwitchAppOnOff);
}

/***************************************************************************//*!
*
* @brief   Returns if in run state
*
* @param   void
*
* @return  none
*
******************************************************************************/
bool M1_IsRunning(void)
{
	return (gsM1_Ctrl.eState == RUN);
}

/***************************************************************************//*!
*
* @brief   Set the speed command function
*
* @param   void
*
* @return  none
*
******************************************************************************/
void M1_SetSpeed(Frac16 f16SpeedCmd)
{
	if (mbM1_SwitchAppOnOff)
	{
		if (f16SpeedCmd < gsM1_Drive.sFaultThresholds.f16SpeedUnder)
		{
			gsM1_Drive.sSpeedCtrl.f32SpeedCmd = 0;
		}
		else if (f16SpeedCmd > gsM1_Drive.sFaultThresholds.f16SpeedOver)
		{
			gsM1_Drive.sSpeedCtrl.f32SpeedCmd = 0;
		}
		else
		{
			gsM1_Drive.sSpeedCtrl.f32SpeedCmd = f16SpeedCmd;	
		}
	}
	else
	{
		gsM1_Drive.sSpeedCtrl.f32SpeedCmd = 0;
	}
}

/***************************************************************************//*!
*
* @brief   Fault function
*
* @param   void
*
* @return  none
*
******************************************************************************/
void M1_Fault(void)
{
	M1_FaultDetection();

	if (MC_FAULT_ANY(gsM1_Drive.sFaultId))
	{
		/* Fault state */
		gsM1_Ctrl.uiCtrl |= SM_CTRL_FAULT;
	}
}

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