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

/******************************************************************************
* Includes
******************************************************************************/
#include "PE_types.h"
#include "gflib.h"
#include "gmclib.h"
#include "gdflib.h"
#include "mlib.h"
#include "intrinsics_56800E.h"
#include "intrinsics_56800EX.h"
#include "dual_servo.h"

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

/******************************************************************************
* Macros 
******************************************************************************/
/* Alignment */
#define ALIGN_METHOD					1	// indicate to which alignment method to be use


/******************************************************************************
* Types
******************************************************************************/
/*	==========================================================================================
 * 
 *   IA: ANAx	IC: ANAx
 *   IB: ANBx	IC: ANBx
 *   
 *   The result of ADC0 & ADC1 will be read out in the ISR where state-machine is executed.
 *   e.g.
 *   SVM Sector:
 *   1&6 ---> gsM1_I.f16A is IC
 *   		  gsM1_I.f16B is IB
 *   2&3 ---> gsM1_I.f16A is IA
 *   		  gsM1_I.f16B is IC
 *   4&5 ---> gsM1_I.f16A is IA
 *   		  gsM1_I.f16B is IB
 *  ==========================================================================================*/
typedef struct
{
    Frac16					f16Ph0;		/* Phase 0 DC offset */
    Frac16					f16Ph1;		/* Phase 1 DC offset */
    Frac16					f16Ph2;		/* Phase 2 DC offset */
    Frac16					f16Ph3;		/* Phase 3 DC offset */
    Frac16					f16Idcb;	/* Dc bus current DC offset */
} MCSTRUC_ADC_CURRENT_CH_T;

typedef struct
{
    GDFLIB_FILTER_MA_T_A32    	sPh0;     /* DC offset filter structure measured on phase A current */
    GDFLIB_FILTER_MA_T_A32    	sPh1;     /* DC offset filter structure measured on phase B current */
    GDFLIB_FILTER_MA_T_A32    	sPh2;     /* DC offset filter structure measured on phase C current */
    GDFLIB_FILTER_MA_T_A32    	sPh3;     /* DC offset filter structure measured on phase secondary C current */
    GDFLIB_FILTER_MA_T_A32    	sIdcb;    /* DC offset filter structure measured on DC bus current */
	MCSTRUC_ADC_CURRENT_CH_T	sI;
} MCSTRUC_ADC_OFFSET_T;

typedef struct
{
    UWord16					uw16Ph0;		/* Phase 0 channel number */
    UWord16					uw16Ph1;		/* Phase 1 channel number */
} MCSTRUC_ADC_2_PHASE_ASSIGNMENT_T;

typedef struct
{
	Frac16					f16Angle;
	UWord16					uw16CCW;
	UWord16					uw16CW;
} MCSTRUC_HALL_DECODER_T;

typedef struct
{
    Frac32						f32Position;			/* Position of field at alignment, executed in current loop */
    Frac32						f32U;					/* D voltage at alignment, f32U += f32UStep */
    Frac32						f32Id;					/* D current at alignment, f32Id += f32IdStep */
    Frac16						f16Id;					/* D current at alignment */
    Frac32						f32Speed;				/* Speed of field at alignment */
    Frac32						f32UStep;				/* D voltage ramp at alignment */
    Frac32						f32IdStep;				/* D voltage ramp at alignment */
    Frac16						f16IMax;				/* Max D current at alignment */
    Frac16						f16UMax;				/* Max D voltage at alignment */
    UWord16						uw16TimeAlignment;		/* Alignment time duration */
} MCSTRUC_ALIGNMENT_T;

typedef struct
{
    GFLIB_CTRL_PI_P_AW_T_A32  		sIdPiParams;		/* Id PI controller parameters */
    GFLIB_CTRL_PI_P_AW_T_A32  		sIqPiParams;		/* Iq PI controller parameters */
    GDFLIB_FILTER_IIR1_T_F32		sIdZcFilter;		/* D current zero-cancellation filter */
    GDFLIB_FILTER_IIR1_T_F32		sIqZcFilter;		/* Q current zero-cancellation filter */
    GDFLIB_FILTER_IIR1_T_F32		sUDcBusFilter;	/* Dc bus voltage filter */
    GMCLIB_3COOR_T_F16    			sIABC;			/* Measured 3-phase current */
    GMCLIB_2COOR_ALBE_T_F16			sIAlBe;   		/* Alpha/Beta current */
    GMCLIB_2COOR_DQ_T_F16			sIDQ;     		/* DQ current */
    GMCLIB_2COOR_DQ_T_F16			sIDQReq;  		/* DQ required current */
    GMCLIB_2COOR_DQ_T_F16			sIDQReqZc;  		/* DQ required current after zero cancellation */
    GMCLIB_2COOR_DQ_T_F16			sIDQError;  		/* DQ current error */
    GMCLIB_3COOR_T_F16    			sDutyABC;			/* Applied duty cycles ABC */
    GMCLIB_2COOR_ALBE_T_F16			sUAlBeReq;   		/* Required Alpha/Beta voltage */
    GMCLIB_2COOR_ALBE_T_F16			sUAlBeComp;	 	/* Compensated to DC bus Alpha/Beta voltage */
    GMCLIB_2COOR_DQ_T_F16			sUDQReq;     		/* Required DQ voltage */
    GMCLIB_2COOR_DQ_T_F16			sUDQController; 	/* Required DQ voltage */
	GMCLIB_2COOR_SINCOS_T_F16		sAnglePosEl; 		/* Electrical position sin/cos (at the moment of PWM current reading) */
	GMCLIB_2COOR_SINCOS_T_F16		sAnglePosElUpdate;/* Compensated electrical position sin/cos (at the moment of PWM update) */
    MCSTRUC_ALIGNMENT_T				sAlignment;		/* Alignment structure params */
	UWord16 						uw16SectorSVM;		/* SVM sector */
	Frac16							f16DutyCycleLimit;	/* Max. allowable duty cycle in frac */
	Frac16							f16UDcBus;			/* DC bus voltage */
	Frac16							f16UDcBusFilt;		/* Filtered DC bus voltage */
	bool_t 					        bIdPiSatFlag;		/* Id PI controller saturation flag */
	bool_t 					        bIqPiSatFlag;		/* Iq PI controller saturation flag */
	bool_t							bOpenLoop;			/* Current control loop is open */
	bool_t							bUseMaxBus;			/* Calculate the max. possible DQ current controllers' output limits based on dc bus voltage */
	bool_t							bUseZc;				/* User zero-cancellation filter */
} MCSTRUC_FOC_PMSM_T;

typedef struct
{
	IIR32_FILTER_T					sSpeedFilter;		/* Speed filter */				
    MCSTRUC_SPEED_ANTI_WINDUP_PI_T  sSpeedPIController;	/* Speed PI controller  */
	GFLIB_RAMP_T_F32				sSpeedRampParams; 	/* Speed ramp parameters */
	Frac32							f32Speed;			/* Raw Speed */
	Frac32							f32SpeedFilt;		/* Speed filtered */
	Frac32							f32SpeedError;		/* Speed error */
	Frac32							f32SpeedRamp;		/* Required speed (ramp output) */
	Frac32							f32SpeedReq;		/* Required speed (ramp input) */
	Frac32							f32SpeedCmd;		/* Speed command (entered by user or master layer) */
	bool_t							bOpenLoop;			/* Speed control loop is open */
} MCSTRUC_SPEED_CTRL_T;

typedef struct
{
	GMCLIB_2COOR_SINCOS_T_F16		sAnglePosEl; 				/* Electrical position sin/cos (at the moment of PWM current reading) */
	GMCLIB_2COOR_SINCOS_T_F16		sAnglePosElUpdate;			/* Compensated electrical position sin/cos (at the moment of PWM update) */
    Word16							w16PolePairs;				/* No. of pole-pairs */
    UWord16							uw16PositionMechRaw;		/* Raw position */
    UWord16							uw16PositionMechRawMask;	/* Mask to filter out the position bits of the counter */
    Frac16							f16PositionMechScale;		/* Scale to recalculate the raw position to fractional mechanical position */
    Word16							w16PositionMechScaleShift;	/* Scale shift (to the left) to recalculate the raw position to fractional mechanical position */
    Frac16							f16PositionMech;			/* Fractional mechanical position (in the moment of reading)*/
    Frac16							f16PositionEl;				/* Fractional electrical position (in the moment of reading)*/
    Frac16							f16PositionElUpdate;		/* Fractional electrical position (in the moment of PWM update)*/
    Word16							w16EncPulsesRevMech;		/* No. of encoder pulses per mech. revolution */

    IIR32_FILTER_T					sSpeedFilter;				/* Speed filter */				

	Frac16							f16Speed;					/* Speed of 16-bit resolution */
	Frac32							f32Speed;					/* Speed of 32-bit resolution */
	Frac16							f16SpeedImproved;			/* improved resolution during low speed */
	Frac32							f32SpeedImproved;			/* improved resolution during low speed */
	Frac16							f32SpeedFilt;				/* Speed filtered */
} MCSTRUC_POS_SPEED_ENCODER_T;

typedef struct
{
	Word16 							w16ZCnt;	   			// value of index counter
	Word16 							w16ZCnt_1;				// value of last time index counter
	Word16 							w16ZCntInc;				// increment between Zcnt and Zcnt_1
	Word16 							w16RealZCnt;			// real rounds in positive direction
	Word16 							w16EncCnt;  			// value of encoder counter
	Word16 							w16EncCnt_1;			// value of last time encoder counter
	Word16 							w16EncCntInit; 			// initial encoder value at start
	Word16 							w16EncInc;				// increment between EncCnt and EncCnt_1
	Word16 							w16FlagDirection;		// flag indicating that direction of rotation
	Frac32 							f32CoeffZ;  			// coefficient for index counter
	Frac32 							f32CoeffEnc;			// coefficient for enc counter
	Frac32 							f32PosRelative;  		// the rotor relative position
//	Frac32 							f32RelativePos;			// diff between CurPos and InitPos
//	Frac32 							f32HallPosBias;			// position bias is caused by hall interrupt
//	Word16 							w16EncIncHall;			// increment between EncCnt and EncCnt_1 in hall interrupt
} MCSTRUC_POS_CAlC_ENCODER_T;

typedef struct
{
	MCSTRUC_POS_CONTROLLER_T		sPosController;			// position Kp controller
	GFLIB_RAMP_T_F32				PosRampInc;				// ramp function: desired position value --> position value after ramp
	SMOOTH_FILTER_STRUCT			udtPosSmoothFilter;		// smooth filter for position loop
	Frac32							f32PosDesired;			// desired position value
	Frac32							f32PosRamp;				// position value after ramp
	Frac32							f32PosSmooth;			// position value after smooth filter
	Frac32 							f32PError;				// position error
	Word16							w16FlagSineTest;		// flag indicating to do sine wave testing
	Frac32							f32SineAmplitude;		// Amplitude of position sine wave
	Word16							w16FlagPosFilter;	/* flag indicating which filter would be used
																	0: f32PosDesired --> f32PosRamp --> f32PosSmooth --> f32PError
																	1: f32PosDesired --> f32PosRamp --> f32PError
															 	 	2: f32PosDesired --> f32PosSmooth --> f32PError
																 	3: f32PosDesired --> f32PError  for sine wave testing
															*/	
} MCSTRUC_POS_CTRL_T;

typedef struct
{
	Word16 w16BaseFreq;		 // base frequency in Hz
	Word16 w16ControlFreq;   // control loop frequency in Hz
	Frac16 f16Tmp;			 // this is (w16BaseFreq/w16ControlFreq)	     
	Frac32 f32DesiredFreq;	 // the expecting frequency of angle increasing between 0~360 degrees in per-unit
	Frac32 f32Delta;		 // the increment of angle in per-unit
	Frac32 f32StartAngle;	 // start angle in per-unit
	Frac32 f32CurAngle;		 // current angle in per-unit
} ANGLE_GENERATOR_T;

typedef struct
{
	UWord16			uw16U;
	UWord16			uw16V;
	UWord16			uw16W;	
	UWord16			uw16Num;
	UWord16			uw16Num_1;
	UWord16			uw16State;
	Frac16			f16PositionOrigin;
	Frac16			f16PositionTarget;
	Frac16			f16IdDesired;
	Frac16			f16IdRamp;
	UWord16			uw16AngleToEncoder;
} MCSTRUC_HALL_T;

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

/******************************************************************************
* Global functions
******************************************************************************/
extern void MCSTRUC_FocPMSMCurrentCtrl(MCSTRUC_FOC_PMSM_T *psFocPMSM);
extern void MCSTRUC_AngleFromEncoder(MCSTRUC_POS_SPEED_ENCODER_T *psAngle);

extern void MCSTRUC_Current4ChannelCalibration(MCSTRUC_ADC_CURRENT_CH_T *psCurrent, MCSTRUC_ADC_OFFSET_T *psOffset);
extern void MCSTRUC_Current2ChannelUpdateOffset(UWord16 uw16SVMSectorParam, GFLIB_VECTORLIMIT_T_F16 *psCurrent, MCSTRUC_ADC_OFFSET_T *psOffset);
extern void MCSTRUC_AlignmentPMSM(MCSTRUC_FOC_PMSM_T *psFocPMSM);
//extern void MCSTRUC_SpdCtrlAntiWindupInit(MCSTRUC_SPEED_ANTI_WINDUP_PI_T *ptr);

extern void MCSTRUC_PosCalc(MCSTRUC_POS_CAlC_ENCODER_T *ptr);
extern Frac32 MCSTRUC_PosControllerCalc(Frac32 f32Err, MCSTRUC_POS_CONTROLLER_T *ptr);
extern void AngleCalculation(ANGLE_GENERATOR_T *ptr);

#endif /* _MCSTRUC_H_ */
