#include "mc9s12zvml128.h"
#include "S12zvm_periph.h"
#include "freemaster.h"
#include "state_machine.h"
#include "mlib.h"
#include "gflib.h"
#include "gmclib.h"
#include "gdflib.h"
#include "motor_structure.h"
#include "PMSMFOC_appconfig.h"
#include "S12ZVM_devconfig.h"
#include "meas_s12zvm.h"
#include "actuate_s12zvm.h"
#include "bemf_Observer.h"

/*****************************************************************************
*
* Definitions
*
*****************************************************************************/

/*****************************************************************************
*
* Variables Definition
*
*****************************************************************************/
pmsmDrive_t         drvFOC;         // Field Oriented Control Variables
driveStates_t 		cntrState;	    // Responsible for stateMachine state propagation
appFaultStatus_t	tempfaults;		// Temporary faults to be indicated inhere
appFaultStatus_t	permFaults;		// Permanent faults to be indicated inhere
fm_scale_t			fmScale;		// Scales for freemaster interface
measModule_t		meas;
tPos_mode			pos_mode;

static tBool focFastLoop(void);
static tBool focSlowLoop(void);
static tBool faultDetection();

tBool automaticMode(void);
static tBool CalcOpenLoop(openLoopPospe_t *openLoop, tFrac16 speedReqRamp);
static tBool CalcSensorless(sensorLessPospe_t *sensorless, SWLIBS_2Syst_F16 *iAlBeFbck, SWLIBS_2Syst_F16 *uAlBeReq);

// Interrupts
INTERRUPT void TIMchan0_ISR();
INTERRUPT void PTUTrigger0Done_ISR(void);
INTERRUPT void PTUTrigger1Done_ISR(void);
INTERRUPT void ADC0error_ISR(void);
INTERRUPT void ADC1error_ISR(void);
INTERRUPT void ADC0done_ISR(void);
INTERRUPT void ADC1done_ISR(void);
INTERRUPT void PMFreloadA_ISR(void);

// Debug variables
static tFrac16			reload_PmfCnt;
volatile unsigned int 	AdcErrorLDOK, AdcErrorRSTAR, AdcErrorTRIG, AdcErrorEOL, AdcErrorCMD, AdcErrorIA;
volatile unsigned int 	AdcError0, AdcError1, TrigError0, TrigError1;
// End of debug variables


/*****************************************************************************
*
* Function: void main(void)
*
* Description: main function
*
*****************************************************************************/

void main(void)
{
  cpmu_init();
  pim_init();
  pmf_init();
  ptu_init();
  adc0_init();
  adc1_init();
  sci1_init();
  gdu_init();

  FMSTR_Init();

  cntrState.state   = init;
  cntrState.event = e_init;

  EnableInterrupts;

  for(;;)
  {

	
    FMSTR_Poll();
  }
}


/*****************************************************************************
*
* Function: void ADC0error_ISR(void)
*
* Description: ADC0 Error Interrupt Service Routine
*
*****************************************************************************/
INTERRUPT void ADC0error_ISR(void)
{
char tmpAdcEIF;


  EnableInterrupts;
  AdcError0++;
  tmpAdcEIF = ADC0EIF;

  if (tmpAdcEIF & 0x02) {       // Load Ok Error
      AdcErrorLDOK++;
      ADC0EIF = 0x02;
  }
  if (tmpAdcEIF & 0x04) {       // Restart Request Error
      AdcErrorRSTAR++;
      ADC0EIF = 0x04;
  }
  if (tmpAdcEIF & 0x08) {       // Trigger Error => Soft Reset
      AdcErrorTRIG++;
      ADC0CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x20) {       // End Of List Error => Soft Reset
      AdcErrorEOL++;
      ADC0CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x40) {       // Command Value Error => Soft Reset
      AdcErrorCMD++;
      ADC0CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x80) {       // Illegal Access Error => Soft Reset
      AdcErrorIA++;
      ADC0CTL_0_ADC_SR = 1;
  }
}

/*****************************************************************************
*
* Function: void ADC1error_ISR(void)
*
* Description: ADC1 Error Interrupt Service Routine
*
*****************************************************************************/
INTERRUPT void ADC1error_ISR(void)
{
char tmpAdcEIF;

  EnableInterrupts;
  AdcError1++;
  tmpAdcEIF = ADC1EIF;

  if (tmpAdcEIF & 0x02) {       // Load Ok Error
      AdcErrorLDOK++;
      ADC1EIF = 0x02;
  }
  if (tmpAdcEIF & 0x04) {       // Restart Request Error
      AdcErrorRSTAR++;
      ADC1EIF = 0x04;
  }
  if (tmpAdcEIF & 0x08) {       // Trigger Error => Soft Reset
      AdcErrorTRIG++;
      ADC1CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x20) {       // End Of List Error => Soft Reset
      AdcErrorEOL++;
      ADC1CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x40) {       // Command Value Error => Soft Reset
      AdcErrorCMD++;
      ADC1CTL_0_ADC_SR = 1;
  }
  if (tmpAdcEIF & 0x80) {       // Illegal Access Error => Soft Reset
      AdcErrorIA++;
      ADC1CTL_0_ADC_SR = 1;
  }
}


/*****************************************************************************
*
* Function: void ADC0done_ISR(void)
*
* Description: ADC0 Conversion Done Interrupt Service Routine
*
*****************************************************************************/
INTERRUPT void ADC0done_ISR(void)
{
	EnableInterrupts;
	ADC0CONIF = 1;      // Clear flag
}

/***************************************************************************//*!
*
* @brief   ADC interrupt service routine for Current loop of FOC
*
* @param   void
*
* @return  none
*
******************************************************************************/
INTERRUPT void ADC1done_ISR(void)
{
	static tBool getFcnStatus;

	EnableInterrupts;

	// Read the user control interface
	cntrState.usrControl.btSpeedUp   = PTP_PTP1;
	cntrState.usrControl.btSpeedDown = PTS_PTS0;
	cntrState.usrControl.btFlipFlop  = PTT_PTT1;
	
	 // User accessible switch for stopping the application.
	if (cntrState.usrControl.btFlipFlop ^ cntrState.usrControl.btFlipFlopTemp)
	{
		cntrState.usrControl.btFlipFlopTemp  = cntrState.usrControl.btFlipFlop;
		cntrState.usrControl.switchAppOnOff  = (cntrState.usrControl.btFlipFlop) ? false: true;
	}

	 // User accessible switch for stopping the application.
	 if (cntrState.usrControl.switchAppOnOff ^ cntrState.usrControl.switchAppOnOffState)
	 {
		 cntrState.usrControl.switchAppOnOffState  = cntrState.usrControl.switchAppOnOff;
		 cntrState.event   = (cntrState.usrControl.switchAppOnOff) ? e_app_on: e_app_off;
	 }
						
	getFcnStatus    =    true;

	// Phase currents and DCB voltage measurement
	getFcnStatus  = Meas_Get3PhCurrent(&meas, &drvFOC.iAbcFbck, drvFOC.svmSector);
	getFcnStatus &= Meas_GetTemperature(&meas);
	getFcnStatus &= Meas_GetUdcVoltage(&meas, &drvFOC.uDcbFilter);
	

	drvFOC.f16Udcb = meas.measured.f16Udcb.filt;
	   
    // Fault detection routine, must be executed prior application state machine
	getFcnStatus &= faultDetection();
	if (getFcnStatus)    cntrState.event = e_fault;

	// Execute State table with newly measured data
    state_table[cntrState.event][cntrState.state]();
    state_LED[cntrState.state]();
        
    FMSTR_Recorder();

    ADC1CONIF_1_CON_IF1 = 1; // Clear flag
}




/***************************************************************************//*!
*
* @brief   FAULT state
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateFault()
{
    /*-----------------------------------------------------
    Application State Machine - state identification
    ----------------------------------------------------- */
    // Entering state fault
	cntrState.state   = fault;
	cntrState.event   = e_fault;

	// Turn off Actuator's output (PWM, GDU)
	DisableOutput();

    // Disable user application switch
    cntrState.usrControl.switchAppOnOff      = false;
    cntrState.usrControl.switchAppOnOffState = false;
    
    // Clear fault by on board user buttons.
    // Simultaneous press clearing faults
   /* if (!cntrState.usrControl.btSpeedUp && 
    	!cntrState.usrControl.btSpeedDown)
        cntrState.usrControl.switchFaultClear = 1;*/

    if (cntrState.usrControl.switchFaultClear && ClearDriverError())
    {
        // Clear all of the HW/MCU faults such as GDU, PWM, ...

    	// Clear permanent and temporary SW faults
        permFaults.mcu.R          = 0x0;      // Clear mcu faults
        permFaults.motor.R		  = 0x0;	  // Clear motor faults
        permFaults.stateMachine.R = 0x0;	  // Clear state machine faults

		// When all Faults cleared prepare for transition to next state.
        cntrState.usrControl.readFault             = true;
        cntrState.usrControl.switchFaultClear      = false;
        cntrState.event                            = e_fault_clear;
    }
}

/***************************************************************************//*!
*
* @brief   INIT state
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateInit( )
{
    /*-----------------------------------------------------
    Application State Machine - state identification
    ----------------------------------------------------- */
    tBool InitFcnStatus;
    cntrState.state                              = init;
    cntrState.event								 = e_init;

    //Switch off PWM output, GDU output
    DisableOutput();

    /*------------------------------------
     * General use variables
     * ----------------------------------*/
    InitFcnStatus                                         = false;

    /*------------------------------------
     * Application state machine variables
     * ----------------------------------*/
    // Reset state of all user control variables
    cntrState.usrControl.switchAppOnOff                      = false;
    cntrState.usrControl.switchAppOnOffState                 = false;
    cntrState.usrControl.switchFaultClear                    = false;
    cntrState.usrControl.switchAppReset                      = false;
    
    cntrState.usrControl.ledCounter							 = 0;
    cntrState.usrControl.ledFlashing						 = 1250;


    drvFOC.pospeControl.speedLoopCntr                       = 0;

    /*------------------------------------
     * Freemaster variables
     * ----------------------------------*/
    fmScale.current                                      = FM_I_SCALE;
    fmScale.voltage                                      = FM_U_SCALE;
    fmScale.dcb_voltage                                  = FM_UDCB_SCALE;
    fmScale.speed_w_e                                    = FM_SPEED_RAD_EL_SCALE;
    fmScale.speed_n_m                                    = FM_SPEED_RMP_MECH_SCALE;
    fmScale.position                                     = FM_POSITION_DEG_SCALE;
    fmScale.speed_ramp									 = FM_SPEED_RAMP_RPM_SCALE;


    InitFcnStatus = Meas_Clear(&meas);
    meas.param.u16CalibSamples                         = 8;    // number of samples = 2^u16CalibSamples
    meas.offset.f16PhA.filtParam.u16NSamples		   = meas.param.u16CalibSamples;
	meas.offset.f16PhB.filtParam.u16NSamples		   = meas.param.u16CalibSamples;
	meas.offset.f16PhC.filtParam.u16NSamples		   = meas.param.u16CalibSamples;


    drvFOC.alignCntr								   = ALIGN_DURATION;

    /*------------------------------------
     * Currents
     * ----------------------------------*/
    drvFOC.iAbcFbck.f16Arg1                = 0;
    drvFOC.iAbcFbck.f16Arg2                = 0;
    drvFOC.iAbcFbck.f16Arg3                = 0;

    drvFOC.iAlBeFbck.f16Arg1               = 0;
    drvFOC.iAlBeFbck.f16Arg2               = 0;

    drvFOC.iDQReq.f16Arg1                  = 0;
    drvFOC.iDQReq.f16Arg2                  = 0;

    drvFOC.iDQReqZC.f16Arg1                = 0;
    drvFOC.iDQReqZC.f16Arg2                = 0;

    drvFOC.iDQErr.f16Arg1                  = 0;
    drvFOC.iDQErr.f16Arg2                  = 0;

    drvFOC.iDQFbck.f16Arg1                 = 0;
    drvFOC.iDQFbck.f16Arg2                 = 0;

    /*------------------------------------
     * Voltages
     * ----------------------------------*/
    drvFOC.uAlBeReq.f16Arg1                = 0;
    drvFOC.uAlBeReq.f16Arg2                = 0;

    drvFOC.uAlBeReqDCB.f16Arg1             = 0;
    drvFOC.uAlBeReqDCB.f16Arg2             = 0;

    drvFOC.uDQReq.f16Arg1                  = 0;
    drvFOC.uDQReq.f16Arg2                  = 0;

    /*------------------------------------
     * Speed/Position
     * ----------------------------------*/
    drvFOC.thTransform.f16Arg1             = 0;
    drvFOC.thTransform.f16Arg2             = 0;

    /*------------------------------------
     * SVC-PWM variables
     * ----------------------------------*/
    drvFOC.svmSector                       = 1;

    drvFOC.pwm16.f16Arg1                   = 0;
    drvFOC.pwm16.f16Arg2                   = 0;
    drvFOC.pwm16.f16Arg3                   = 0;

    /*------------------------------------
     * FOC variables
     * ----------------------------------*/

    // D-axis PI controller
    drvFOC.dAxisPI.f32Acc                  = 0;
    drvFOC.dAxisPI.f16CC1sc                = D_CC1SC;
    drvFOC.dAxisPI.f16CC2sc                = D_CC2SC;
    drvFOC.dAxisPI.f16InErrK1              = 0;
    drvFOC.dAxisPI.f16LowerLimit           = FRAC16(-1.0);
    drvFOC.dAxisPI.f16UpperLimit           = FRAC16(1.0);
    drvFOC.dAxisPI.u16NShift               = D_NSHIFT;

    // Q-axis PI controller
    drvFOC.qAxisPI.f32Acc                  = 0;
    drvFOC.qAxisPI.f16CC1sc                = Q_CC1SC;
    drvFOC.qAxisPI.f16CC2sc                = Q_CC2SC;
    drvFOC.qAxisPI.f16InErrK1              = 0;
    drvFOC.qAxisPI.f16LowerLimit           = FRAC16(-1.0);
    drvFOC.qAxisPI.f16UpperLimit           = FRAC16(1.0);
    drvFOC.qAxisPI.u16NShift               = Q_NSHIFT;

    // DCBus 1st order filter; Fcut = 100Hz, Ts = 100e-6
    drvFOC.uDcbFilter.u16NSamples = 8;
    GDFLIB_FilterMAInit(&drvFOC.uDcbFilter);
    drvFOC.uDcbFilter.f32Acc = FRAC32(0.5);

    drvFOC.elimDcbRip.f16ModIndex          = FRAC16(0.866025403784439);
    drvFOC.elimDcbRip.f16ArgDcBusMsr       = 0;

    drvFOC.speedRamp.f32RampUp             = SPEED_RAMP_UP;
    drvFOC.speedRamp.f32RampDown           = SPEED_RAMP_DOWN;
    drvFOC.speedRamp.f32State              = 0;

    drvFOC.speedPI.f16PropGain             = SPEED_PI_PROP_GAIN;
    drvFOC.speedPI.s16PropGainShift        = SPEED_PI_PROP_GAIN_SHIFT;
    drvFOC.speedPI.f16IntegGain            = SPEED_PI_INTEG_GAIN;
    drvFOC.speedPI.s16IntegGainShift       = SPEED_PI_INTEG_GAIN_SHIFT;
    drvFOC.speedPI.f32IntegPartK_1         = 0;
    drvFOC.speedPI.f16UpperLimit           = FRAC16(MOTOR_I_PH_NOM/I_MAX);
    drvFOC.speedPI.f16LowerLimit           = FRAC16(-MOTOR_I_PH_NOM/I_MAX);

    // Position observer
    drvFOC.pospeSensorless.wRotEl   	   					= 0;
    drvFOC.pospeSensorless.thRotEl		   					= 0;
    drvFOC.pospeSensorless.bEMFObs.DQtoGaDeError			= 0;
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f32Acc          = 0;
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f16CC1sc        = D_CC1SC;
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f16CC2sc        = D_CC2SC;
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f16InErrK1      = 0;
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f16LowerLimit   = FRAC16(-1.0);
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.f16UpperLimit   = FRAC16(1.0);
    drvFOC.pospeSensorless.bEMFObs.PI_Ga_Emf.u16NShift       = D_NSHIFT;


    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f32Acc          = 0;
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f16CC1sc        = D_CC1SC;
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f16CC2sc        = D_CC2SC;
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f16InErrK1      = 0;
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f16LowerLimit   = FRAC16(-1.0);
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.f16UpperLimit   = FRAC16(1.0);
    drvFOC.pospeSensorless.bEMFObs.PI_De_Emf.u16NShift       = D_NSHIFT;

    drvFOC.pospeSensorless.bEMFObs.Kscale_n 	= Kscale_SlsPlant;
    drvFOC.pospeSensorless.bEMFObs.K1n			= K1_SlsPlant;
    drvFOC.pospeSensorless.bEMFObs.K2n			= K2_SlsPlant;
    drvFOC.pospeSensorless.bEMFObs.K3n			= K3_SlsPlant;
    drvFOC.pospeSensorless.bEMFObs.K4n			= K4_SlsPlant;

    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.f32IntegPartK_1 		= 0;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.u16LimitFlag       	= 0;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.f16PropGain  		= TO_KP_GAIN;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.f16IntegGain 		= TO_KI_GAIN;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.s16PropGainShift 	= TO_KP_SCALE;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.s16IntegGainShift 	= TO_KI_SCALE;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.f16UpperLimit 		= FRAC16(0.9999);
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs.f16LowerLimit 		= FRAC16(-0.9999);

    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f32Acc          = 0;
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f16CC1sc        = TO_CC1SC;
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f16CC2sc        = TO_CC2SC;
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f16InErrK1      = 0;
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f16LowerLimit   = FRAC16(-1.0);
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f16UpperLimit   = FRAC16(1.0);
	drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.u16NShift       = TO_NSHIFT;

    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.filterMA.f32Acc 		= 0;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.filterMA.u16NSamples 	= (2);

    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.f16InK1 	= 0;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.f32State 	= 0;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.f16C1 		= TO_INTEG_C1;
    drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.u16NShift 	= TO_INTEG_NSHIFT;

    drvFOC.pospeSensorless.wRotEl			= 0;
    drvFOC.pospeSensorless.thRotEl			= 0;
    drvFOC.pospeSensorless.wRotElMatch	= MATCHING_W_EL;
    
    drvFOC.pospeSensorless.iQUpperLimit		= CUR_LIM_SENSORLESS;
    drvFOC.pospeSensorless.iQLowerLimit		= MLIB_Neg_F16(drvFOC.pospeSensorless.iQUpperLimit);

    drvFOC.pospeOpenLoop.integ.f16InK1 		= 0;
    drvFOC.pospeOpenLoop.integ.f32State 	= 0;
    drvFOC.pospeOpenLoop.integ.f16C1 		= TO_INTEG_C1;
    drvFOC.pospeOpenLoop.integ.u16NShift 	= TO_INTEG_NSHIFT;

    drvFOC.pospeOpenLoop.thRotEl			= 0;
    drvFOC.pospeOpenLoop.wRotEl				= 0;

    drvFOC.pospeOpenLoop.iQUpperLimit		= CUR_LIM_OPENLOOP;
    drvFOC.pospeOpenLoop.iQLowerLimit		= MLIB_Neg_F16(drvFOC.pospeOpenLoop.iQUpperLimit);
    
    // Default mode of operation
    cntrState.usrControl.controlMode		= automatic;
    pos_mode								= force;

    if (!InitFcnStatus)
    {
    	// Fault in initialisation state
        tempfaults.stateMachine.B.InitError = 1;		// Mark the initialisation fault
        cntrState.event                     = e_fault;	// prepare for transition to fault state.
    }
    else
    {
    	// Initialisation phase successfully done
    	cntrState.event   = e_init_done;  // prepare for transition to next state.
    }
}

/***************************************************************************//*!
*
* @brief   READY state
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateReady()
{
    /*-----------------------------------------------------
    Application State Machine - state identification
    ----------------------------------------------------- */
    cntrState.state   = ready;
    cntrState.event   = e_ready;
}

/***************************************************************************//*!
*
* @brief   CALIBRATION state - ADC calibration state
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateCalib()
{
    /*-----------------------------------------------------
      Application State Machine - state identification
    ----------------------------------------------------- */
    tBool           CalibStatus;

    cntrState.state    = calib;
    cntrState.event    = e_calib;
    CalibStatus        = false;

    // Turn on actuator output
    EnableOutput();

    CalibStatus = Meas_CalibCurrentSense(&meas, drvFOC.svmSector);

    // Apply 0.5 duty cycle
    drvFOC.pwm16.f16Arg1 = FRAC16(0.5);
    drvFOC.pwm16.f16Arg2 = FRAC16(0.5);
    drvFOC.pwm16.f16Arg3 = FRAC16(0.5);

    SetDutycycle(&drvFOC.pwm16);


    // Exit the calibration state when DC calibration is done for all sectors
    if (CalibStatus)
    {
    	// Calibration sequence has successfully finished
		cntrState.event               = e_calib_done;    
    }
}

/***************************************************************************//*!
*
* @brief   ALIGNMENT state - motor control d-axes alignment
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateAlign( )
{
    /*-----------------------------------------------------
    Application State Machine - state identification
    ----------------------------------------------------- */
    tBool           AlignStatus;

    cntrState.state   = align;
    cntrState.event   = e_align;

    // Turn on Actuator output on (PWM,GDU)
    EnableOutput();

    // Align sequence is at the beginning
    AlignStatus     = true;

    drvFOC.uDQReq.f16Arg1      = ALIGN_VOLTAGE;
    drvFOC.uDQReq.f16Arg2      = FRAC16(0);

    drvFOC.thTransform.f16Arg1 = GFLIB_Sin(0);
    drvFOC.thTransform.f16Arg2 = GFLIB_Cos(0);

    GMCLIB_ParkInv(&(drvFOC.uAlBeReq),&(drvFOC.thTransform),&(drvFOC.uDQReq));
    drvFOC.svmSector = GMCLIB_SvmStd(&(drvFOC.pwm16),&(drvFOC.uAlBeReq));

    // when alignment time elapse
    if (--(drvFOC.alignCntr)<=0)
    {
        drvFOC.iDQReq.f16Arg1 = FRAC16(0.0);
        drvFOC.iDQReq.f16Arg2 = FRAC16(0.0);

        drvFOC.uDQReq.f16Arg1 = 0;
        drvFOC.uDQReq.f16Arg2 = 0;

        drvFOC.dAxisPI.f16InErrK1  = 0;
        drvFOC.dAxisPI.f32Acc      = 0;

        drvFOC.qAxisPI.f16InErrK1  = 0;
        drvFOC.qAxisPI.f32Acc      = 0;

        drvFOC.pwm16.f16Arg1 = FRAC16(0.5);
        drvFOC.pwm16.f16Arg2 = FRAC16(0.5);
        drvFOC.pwm16.f16Arg3 = FRAC16(0.5);

        if (!AlignStatus)
        {
        	tempfaults.stateMachine.B.AlignError     = 1;
        }
        else
        {
        	cntrState.event           = e_align_done;
        }
    }

    SetDutycycle(&drvFOC.pwm16);
}

/***************************************************************************//*!
*
* @brief   RUN state
*
* @param   none
*
* @return  none
*
******************************************************************************/
void stateRun( )
{
    static tBool stateRunStatus;

    stateRunStatus = false;
    /*-----------------------------------------------------
    Application State Machine - state identification
    ----------------------------------------------------- */
    cntrState.state   = run;
    cntrState.event   = e_run;

    /*-----------------------------------------------------
        Calculate positions
     ----------------------------------------------------- */
	CalcOpenLoop(&drvFOC.pospeOpenLoop,drvFOC.pospeControl.wRotElReqRamp);
	CalcSensorless(&drvFOC.pospeSensorless,&drvFOC.iAlBeFbck,&drvFOC.uAlBeReq);

	drvFOC.pospeOpenLoop.thDifOpenLEstim = MLIB_Sub_F16(drvFOC.pospeSensorless.thRotEl, drvFOC.pospeOpenLoop.thRotEl);
	
	/*-----------------------------------------------------
	Get positions according to selected mode
	----------------------------------------------------- */
	// Selecting 0 will disable the "AUTOMATIC MODE"
	// where the transition from open loop to sensorless in performed automaticaly
	// Selecting 1 will enable the "USER mode"
	// where user decide whether to switch to force mode, tracking mode, sensorless mode
	if (cntrState.usrControl.controlMode == automatic)
	{
		automaticMode();
	}
	
	// user decide whether to switch to force mode, tracking mode, sensorless mode
	switch (pos_mode)
	{
		case force:
			drvFOC.pospeControl.thRotEl = drvFOC.pospeOpenLoop.thRotEl;
			drvFOC.pospeControl.wRotEl	= 0;
			
			drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.f32Acc = ((tFrac32)drvFOC.pospeOpenLoop.wRotEl << (16 + drvFOC.pospeSensorless.bEMFObs.TrackObsrv.PI_Pospe_Obs_Rec.u16NShift));
			drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.f32State = ((tFrac32)(drvFOC.pospeOpenLoop.thRotEl) << (16 + drvFOC.pospeSensorless.bEMFObs.TrackObsrv.Ki_IntAto.u16NShift));
		break;
		case tracking:
			drvFOC.pospeControl.thRotEl = drvFOC.pospeOpenLoop.thRotEl;
			drvFOC.pospeControl.wRotEl	= 0;	
		break;
		case sensorless1:
			drvFOC.pospeControl.thRotEl = drvFOC.pospeSensorless.thRotEl;
			drvFOC.pospeControl.wRotEl	= drvFOC.pospeSensorless.wRotEl;	
		break;
	}
	

	/*-----------------------------------------------------
	    Calculate Field Oriented Control FOC
	----------------------------------------------------- */
    if (++drvFOC.pospeControl.speedLoopCntr>=SPEED_LOOP_CNTR)
    {
        drvFOC.pospeControl.speedLoopCntr    = 0;

        if (!cntrState.usrControl.btSpeedUp)
            drvFOC.pospeControl.wRotElReq = MLIB_AddSat_F16(drvFOC.pospeControl.wRotElReq, FRAC16(1.0/W_MAX));

        if (!cntrState.usrControl.btSpeedDown)
        	drvFOC.pospeControl.wRotElReq = MLIB_SubSat_F16(drvFOC.pospeControl.wRotElReq,FRAC16(1.0/W_MAX));


        
        stateRunStatus  = focSlowLoop();
        if (!stateRunStatus)
        {
        	tempfaults.stateMachine.B.RunError = 1;
        }
    }

    stateRunStatus = focFastLoop();
        
    if (!stateRunStatus)
    {
    	tempfaults.stateMachine.B.RunError = 1;
    }

    SetDutycycle(&drvFOC.pwm16);
}


/***************************************************************************//*!
*
* @brief   Field Oriented Control - slow loop calculations
*
* @param   
*
* @return  none
*
******************************************************************************/
static tBool focSlowLoop()
{
	drvFOC.pospeControl.wRotElReqRamp = (tFrac16)(GFLIB_Ramp_F32(((tFrac32) drvFOC.pospeControl.wRotElReq)<<16,&drvFOC.speedRamp)>>16);

	drvFOC.pospeControl.wRotEl 		= GDFLIB_FilterMA_F16(drvFOC.pospeControl.wRotEl, &drvFOC.pospeControl.wRotFilter);

	drvFOC.pospeControl.wRotElErr 	= MLIB_SubSat(drvFOC.pospeControl.wRotElReqRamp,drvFOC.pospeControl.wRotEl);
	drvFOC.iDQReq.f16Arg2         	= GFLIB_ControllerPIpAW(drvFOC.pospeControl.wRotElErr,&drvFOC.speedPI);

    return true;
}

/***************************************************************************//*!
*
* @brief   Field Oriented Control - fast loop calculations
*
* @param   
*
* @return  none
*
******************************************************************************/
volatile unsigned int dPI = 1, qPI = 1;
static tBool focFastLoop()
{
	GMCLIB_Clark(&drvFOC.iAlBeFbck,&drvFOC.iAbcFbck);

	drvFOC.thTransform.f16Arg1     = GFLIB_Sin(drvFOC.pospeControl.thRotEl);
	drvFOC.thTransform.f16Arg2     = GFLIB_Cos(drvFOC.pospeControl.thRotEl);

    GMCLIB_Park(&drvFOC.iDQFbck,&drvFOC.thTransform,&drvFOC.iAlBeFbck);

    drvFOC.iDQErr.f16Arg1          = MLIB_SubSat(drvFOC.iDQReq.f16Arg1,drvFOC.iDQFbck.f16Arg1);
    drvFOC.iDQErr.f16Arg2          = MLIB_SubSat(drvFOC.iDQReq.f16Arg2,drvFOC.iDQFbck.f16Arg2);

    // 90% of available DCbus recalculated to phase voltage = 0.90*uDCB/sqrt(3)
    drvFOC.dAxisPI.f16UpperLimit   = (tFrac16)((MLIB_Mul_F32F16F16(FRAC16(0.51961),drvFOC.f16Udcb)) >> 16);
    drvFOC.dAxisPI.f16LowerLimit   = MLIB_Neg(drvFOC.dAxisPI.f16UpperLimit);

    if (dPI)
    {
    	drvFOC.uDQReq.f16Arg1      = GFLIB_ControllerPIrAW(drvFOC.iDQErr.f16Arg1,&drvFOC.dAxisPI);
    }
    	
	// Voltage limit
	drvFOC.qAxisPI.f16UpperLimit   = GFLIB_Sqrt(MLIB_SubSat_F16(
											  MLIB_Mul_F16(drvFOC.dAxisPI.f16UpperLimit, drvFOC.dAxisPI.f16UpperLimit),
											  MLIB_Mul_F16(drvFOC.uDQReq.f16Arg1,drvFOC.uDQReq.f16Arg1)));
	drvFOC.qAxisPI.f16LowerLimit   = MLIB_Neg(drvFOC.qAxisPI.f16UpperLimit);

	if (qPI)
	{
		drvFOC.uDQReq.f16Arg2      = GFLIB_ControllerPIrAW(drvFOC.iDQErr.f16Arg2,&drvFOC.qAxisPI);
	}

    GMCLIB_ParkInv(&drvFOC.uAlBeReq,&drvFOC.thTransform,&drvFOC.uDQReq);

    drvFOC.elimDcbRip.f16ArgDcBusMsr  = meas.measured.f16Udcb.filt;
    GMCLIB_ElimDcBusRip(&drvFOC.uAlBeReqDCB,&drvFOC.uAlBeReq,&drvFOC.elimDcbRip);

    drvFOC.svmSector   = GMCLIB_SvmStd(&(drvFOC.pwm16),&drvFOC.uAlBeReqDCB);

    return (true);
}


/***************************************************************************//*!
*
* @brief   Fault Detection function
*
* @param   none
*
* @return  none
*
******************************************************************************/
static tBool faultDetection()
{
    tBool faultDetectiontEvent;

    faultDetectiontEvent = false;
    //-----------------------------
    // Actual Faults
    //-----------------------------
    // TRIP:   Phase A over-current detected
	tempfaults.motor.B.OverPhaseACurrent = (drvFOC.iAbcFbck.f16Arg1 > FRAC16(I_PH_OVERCURRENT_TRIP/I_MAX)) ? true : false;

	// TRIP:   Phase B over-current detected
	tempfaults.motor.B.OverPhaseBCurrent = (drvFOC.iAbcFbck.f16Arg2 > FRAC16(I_PH_OVERCURRENT_TRIP/I_MAX)) ? true : false;

	// TRIP:   Phase C over-current detected
	tempfaults.motor.B.OverPhaseCCurrent = (drvFOC.iAbcFbck.f16Arg3 > FRAC16(I_PH_OVERCURRENT_TRIP/I_MAX)) ? true : false;

	// TRIP:   DC-bus over-voltage
	tempfaults.motor.B.OverDCBusVoltage  = (meas.measured.f16Udcb.raw > FRAC16(U_DCB_OVERVOLTAGE_TRIP/U_DCB_MAX)) ? true : false;

	// TRIP:   DC-bus under-voltage
	tempfaults.motor.B.UnderDCBusVoltage = (meas.measured.f16Udcb.raw < FRAC16(U_DCB_UNDERVOLTAGE_TRIP/U_DCB_MAX)) ? true : false;

	//-----------------------------
	// Pending Faults
	//-----------------------------

	if (cntrState.state != fault)
	{
		// Fault:   Phase A over-current detected
		permFaults.motor.B.OverPhaseACurrent   = (drvFOC.iAbcFbck.f16Arg1 > FRAC16(I_PH_OVERCURRENT_FAULT/I_MAX)) ? true : false;

		// Fault:   Phase B over-current detected
		permFaults.motor.B.OverPhaseBCurrent   = (drvFOC.iAbcFbck.f16Arg2 > FRAC16(I_PH_OVERCURRENT_FAULT/I_MAX)) ? true : false;

		// Fault:   Phase C over-current detected
		permFaults.motor.B.OverPhaseCCurrent   = (drvFOC.iAbcFbck.f16Arg3 > FRAC16(I_PH_OVERCURRENT_FAULT/I_MAX)) ? true : false;

		// Fault:   DC-bus over-voltage
		permFaults.motor.B.OverDCBusVoltage    = (meas.measured.f16Udcb.raw > FRAC16(U_DCB_OVERVOLTAGE_FAULT/U_DCB_MAX)) ? true : false;

		// Fault:   DC-bus under-voltage
		permFaults.motor.B.UnderDCBusVoltage   = (meas.measured.f16Udcb.raw < FRAC16(U_DCB_UNDERVOLTAGE_FAULT/U_DCB_MAX)) ? true : false;

		// Fault:   Over temperature
		permFaults.motor.B.OverHeating   	   = (meas.measured.f16Temp.filt > FRAC16(TEMP_FAULT/TEMP_MAX)) ? true : false;
	}

	permFaults.mcu.B.GDU_Error = GetDriverError();


    if ((permFaults.motor.R != 0x0))
    	faultDetectiontEvent = true;

    return faultDetectiontEvent;
}


/***************************************************************************//*!
*
* @brief   Open loop generator of open loop position and speed
*
* @param   pointer to structure of openLoopPospe_t type
* @param   speed ramp
*
* @return  none
*
******************************************************************************/
static tBool CalcOpenLoop(openLoopPospe_t *openLoop, tFrac16 speedReqRamp)
{
	openLoop->wRotEl  = speedReqRamp;
	openLoop->thRotEl = GFLIB_IntegratorTR_F16(speedReqRamp,&openLoop->integ);
	return(true);
}


/***************************************************************************//*!
*
* @brief   Calculate Position and Speed In Sensorless Mode
*
* @param   none
*
* @return  bool
*
******************************************************************************/
tBool automaticMode()
{
	if (MLIB_Abs_F16(drvFOC.pospeOpenLoop.wRotEl) < drvFOC.pospeSensorless.wRotElMatch)
	{
		pos_mode = force;
		drvFOC.speedPI.f16UpperLimit = drvFOC.pospeOpenLoop.iQUpperLimit;
	}
	else if (MLIB_Abs_F16(drvFOC.pospeOpenLoop.wRotEl) >= drvFOC.pospeSensorless.wRotElMatch && 
			 MLIB_Abs_F16(drvFOC.pospeOpenLoop.wRotEl) < MLIB_AddSat_F16(drvFOC.pospeSensorless.wRotElMatch,FRAC16(0.05))) 
	{
		pos_mode = tracking;
		drvFOC.speedPI.f16UpperLimit = drvFOC.pospeOpenLoop.iQUpperLimit;
	}
	else
	{
		pos_mode = sensorless1;
		drvFOC.speedPI.f16UpperLimit = drvFOC.pospeSensorless.iQUpperLimit;
	}
	
	
	drvFOC.speedPI.f16LowerLimit = MLIB_Neg_F16(drvFOC.speedPI.f16UpperLimit);
	
	return(true);
}

/***************************************************************************//*!
*
* @brief   Calculate Position E-BEMF observer
*
* @param   structure holding observer parameters and output
* @param   structure holding currents in alfa and beta coordinates
* @param   structure holding applied voltages on motor in alfa and beta coordinates
*
* @return  none
*
******************************************************************************/
static tBool CalcSensorless(sensorLessPospe_t *sensorless, SWLIBS_2Syst_F16 *iAlBeFbck, SWLIBS_2Syst_F16 *uAlBeReq)
{
	ACLIB_PMSMBemfObsrvDQ(&sensorless->wRotEl, &sensorless->thRotEl, iAlBeFbck, uAlBeReq, &sensorless->bEMFObs);
	return(true);
}



/* LED application control*/

/***************************************************************************//*!
*
* @brief   LED OFF state
*
* @param   
*
* @return  none
*
******************************************************************************/
void stateLedOFF()
{
	PTS_PTS1 = 0;
}

/***************************************************************************//*!
*
* @brief   LED ON state
*
* @param   
* 
* @return  none
*
******************************************************************************/
void stateLedON()
{
	PTS_PTS1 = 1;
}

/***************************************************************************//*!
*
* @brief   LED FLASHING state
*
* @param    
* 
* @return  none
*
******************************************************************************/
void stateLedFLASHING_SLOW()
{
	cntrState.usrControl.ledCounter += 1;

	if((cntrState.usrControl.ledCounter)>((cntrState.usrControl.ledFlashing)<<1))
	{
		PTS_PTS1 ^= 1;
		cntrState.usrControl.ledCounter = 0;
	}
}

/***************************************************************************//*!
*
* @brief   LED FLASHING state
*
* @param    
*
* @return  none
*
******************************************************************************/
void stateLedFLASHING_FAST()
{
	cntrState.usrControl.ledCounter += 1;

	if((cntrState.usrControl.ledCounter)>((cntrState.usrControl.ledFlashing)>>1))
	{
		PTS_PTS1 ^= 1;
		cntrState.usrControl.ledCounter = 0;
	}
}
