/******************************************************************************
* 
* 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_def.h
*
* @author    R61928
* 
* @version   1.0.2.0
* 
* @date      May-23-2013
* 
* @brief     Motor control structure
*
*******************************************************************************
*
* Motor control structure.
*
******************************************************************************/
#ifndef _MCDEF_H_
#define _MCDEF_H_

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

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

/******************************************************************************
* Macros 
******************************************************************************/
#define	M1_IA_ADC_0		0		//ANA0
#define M1_IB_ADC_1 	10		//ANB2
#define M1_IC_ADC_0		1		//ANA1
#define M1_IC_ADC_1 	8		//ANB0

#define	M2_IA_ADC_0		4		//ANA4
#define M2_IB_ADC_1 	14		//ANB6
#define M2_IC_ADC_0		5		//ANA5
#define M2_IC_ADC_1 	12		//ANB4

#define MC_FAULT_STARTUP_FAIL 	0   	/* Start-up fail */
#define MC_FAULT_LOAD_OVER 		1		/* Overload Flag */
#define MC_FAULT_I_DCBUS_OVER 	2		/* OverCurrent fault flag */
#define MC_FAULT_U_DCBUS_UNDER 	3	/* Undervoltage fault flag */
#define MC_FAULT_U_DCBUS_OVER 	4		/* Overvoltage fault flag */

//#define FILTER_UDCBUS_B1 	            0.0305	/* DC bus voltage filter B1 */
//#define FILTER_UDCBUS_B2 	            0.0305	/* DC bus voltage filter B2 */
//#define FILTER_UDCBUS_A2 	            -0.9391	/* DC bus voltage filter A2 */

//#define V_DCB_SCALE						36.3 	/* MAX measurable DCB voltage [V] */
//#define V_FOC_SCALE						20.958 	/* V_DCB / SQRT(3) [V] */
//#define I_SCALE							8.052 	/* MAX measurable current [A] */
//#define SPEED_SCALE						FM_NMAX 	/* MAX measurable speed [RPM]*/

//#define I_MAX							8.2		/* Max current [A] */
//#define SPEED_MIN						1.0 	/* MIN applicable speed [RPM]*/
//#define SPEED_MAX						3400.0 	/* Max application speed [RPM] */
//#define OVERVOLT_LIMIT               	35.0 	/* Over-voltage threshold [V] */
//#define UNDERVOLT_LIMIT              	18.0 	/* Under-voltage threshold [V] */

#define OPTION_ALGORITHMS_PRECISION_HIGH	1
/* Sets the fault bit defined by faultid in the faults variable */
#define MC_FAULT_SET(faults, faultid) (faults |= ((MCDEF_FAULT_T)1 << faultid))

/* Clears the fault bit defined by faultid in the faults variable */
#define MC_FAULT_CLEAR(faults, faultid) (faults &= ~((MCDEF_FAULT_T)1 << faultid))

/* Check the fault bit defined by faultid in the faults variable, returns 1 or 0 */
#define MC_FAULT_CHECK(faults, faultid) ((faults & ((MCDEF_FAULT_T)1 << faultid)) >> faultid)

/* Clears all fault bits in the faults variable */
#define MC_FAULT_CLEAR_ALL(faults) (faults = 0)

/* Check if a fault bit is set in the faults variable, 0 = no fault */
#define MC_FAULT_ANY(faults) (faults > 0)

/* Update a fault bit defined by faultid in the faults variable according to the LSB of value */
#define MC_FAULT_UPDATE(faults, faultid, value) {MC_FAULT_CLEAR(faults, faultid); faults |= (((MC_FAULT_T)value & (MC_FAULT_T)1) << faultid);}

#define PWMA_RUN()	setReg16Bits(PWMA_MCTRL, PWMA_MCTRL_RUN0_MASK | PWMA_MCTRL_RUN1_MASK | PWMA_MCTRL_RUN2_MASK)
#define PWMB_RUN()	setReg16Bits(PWMB_MCTRL, PWMB_MCTRL_RUN0_MASK | PWMB_MCTRL_RUN1_MASK | PWMB_MCTRL_RUN2_MASK)

#define M1_DISABLE_PWM_OUTPUT() clrReg16Bits(PWMA_OUTEN, PWMA_OUTEN_PWMA_EN0_MASK | PWMA_OUTEN_PWMB_EN0_MASK | PWMA_OUTEN_PWMA_EN1_MASK | PWMA_OUTEN_PWMB_EN1_MASK | PWMA_OUTEN_PWMA_EN2_MASK | PWMA_OUTEN_PWMB_EN2_MASK)
#define M1_ENABLE_PWM_OUTPUT()  setReg16Bits(PWMA_OUTEN, PWMA_OUTEN_PWMA_EN0_MASK | PWMA_OUTEN_PWMB_EN0_MASK | PWMA_OUTEN_PWMA_EN1_MASK | PWMA_OUTEN_PWMB_EN1_MASK | PWMA_OUTEN_PWMA_EN2_MASK | PWMA_OUTEN_PWMB_EN2_MASK)

#define M2_DISABLE_PWM_OUTPUT() clrReg16Bits(PWMB_OUTEN, PWMB_OUTEN_PWMA_EN0_MASK | PWMB_OUTEN_PWMB_EN0_MASK | PWMB_OUTEN_PWMA_EN1_MASK | PWMB_OUTEN_PWMB_EN1_MASK | PWMB_OUTEN_PWMA_EN2_MASK | PWMB_OUTEN_PWMB_EN2_MASK)
#define M2_ENABLE_PWM_OUTPUT()  setReg16Bits(PWMB_OUTEN, PWMB_OUTEN_PWMA_EN0_MASK | PWMB_OUTEN_PWMB_EN0_MASK | PWMB_OUTEN_PWMA_EN1_MASK | PWMB_OUTEN_PWMB_EN1_MASK | PWMB_OUTEN_PWMA_EN2_MASK | PWMB_OUTEN_PWMB_EN2_MASK)

#define M1_OVERCURRENT_FAULT() (testReg16Bit(PWMA_FSTS0, FFLAG0) > 0) 
#define M2_OVERCURRENT_FAULT() (testReg16Bit(PWMB_FSTS0, FFLAG1) > 0)

#define M1_CLEAR_OVERCURRENT_FAULT()  PeriphSafeClearFlags16(PWMA_FSTS0, PWMA_FSTS0_FFLAG_MASK, PWMA_FSTS0_FFLAG0_MASK)
#define M2_CLEAR_OVERCURRENT_FAULT()  PeriphSafeClearFlags16(PWMB_FSTS0, PWMB_FSTS0_FFLAG_MASK, PWMB_FSTS0_FFLAG1_MASK)

#define M1_MODULO_HALF 3124	// PWMA_SM0VAL1
#define M2_MODULO_HALF 3124 // PWMB_SM0VAL1

#define M1_CLEAR_Z_CNT() setReg16(TMRA_2_CNTR, 0)	// clear M1 Z counter
#define M2_CLEAR_Z_CNT() setReg16(TMRB_2_CNTR, 0)	// clear M2 Z counter

#define M1_READ_Z_CNT(Zcnt) (Zcnt = TMRA_2_CNTR)// read index counter
#define M2_READ_Z_CNT(Zcnt) (Zcnt = TMRB_2_CNTR)

#define M1_READ_ENC_CNT(EncCnt) (EncCnt = TMRA_0_CNTR)// read encoder counter
#define M2_READ_ENC_CNT(EncCnt) (EncCnt = TMRB_0_CNTR)

#define M1_CLEAR_ENC_CNT() setReg16(TMRA_0_CNTR, 0)	// clear M1 encoder counter
#define M2_CLEAR_ENC_CNT() setReg16(TMRB_0_CNTR, 0)	// clear M2 encoder counter

#define M1_WRITE_ENC_CNT(EncCnt) setReg16(TMRA_0_CNTR, EncCnt)// write encoder counter
#define M2_WRITE_ENC_CNT(EncCnt) setReg16(TMRB_0_CNTR, EncCnt)// write encoder counter

#define DISABLE		0
#define ENABLE		1
/******************************************************************************
* Types
******************************************************************************/
typedef UWord16 MCDEF_FAULT_T;

typedef enum {
    FAST           = 0,
    SLOW           = 1
} MCDEF_CONTROL_LOOP_T;         /* Loop */


typedef struct
{
	Frac16					f16UDcBusOver;		/* DC bus over voltage level */
	Frac16					f16UDcBusUnder;		/* DC bus under voltage level */
	Frac16					f16SpeedOver;		/* Over speed level */
	Frac16					f16SpeedUnder;		/* Under speed level */
} MCDEF_FAULT_THRESHOLDS_T;


/* PMSM FOC with Encoder with speed PI controller */
typedef struct
{
	MCDEF_FAULT_T						sFaultId;
	MCDEF_FAULT_T						sFaultIdPending;
	MCDEF_FAULT_THRESHOLDS_T			sFaultThresholds;
	MCSTRUC_ADC_OFFSET_T				sADCOffset;
	MCSTRUC_FOC_PMSM_T					sFocPMSM;
	MCSTRUC_SPEED_CTRL_T				sSpeedCtrl;
	MCSTRUC_SPEED_CALC_T				sSpeedCalc;
	MCSTRUC_POS_SPEED_ENCODER_T			sSpeedPos;
	MCSTRUC_POS_CAlC_ENCODER_T			sPosCalc;
	MCSTRUC_POS_CTRL_T					SPosCtrl;
	MCSTRUC_SEARCH_ROTOR_POS_T			sSearchRotorPos;
	GFLIB_RAMP_T_F16					sSearchRotorRampI;
	MCSTRUC_HALL_T						sHall;
	GFLIB_RAMP_T_F16					sHallRampI;
	UWord16 							uw16FlagSlowLoop;
	UWord16 							uw16CounterSlowLoop;
	UWord16 							uw16DividerSlowLoop;
	UWord16 							uw16CounterState;	// defines how many slow loops that the sub-state lasts for
	UWord16 							uw16TimeFullSpeedFreeWheel;
	UWord16								uw16TimeCalibration;
	UWord16								uw16TimeFaultRelease;
	UWord16								uw16CounterSpdCal;
} MCDEF_FOC_PMSM_ENC_SPEED_PI_T;


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

/******************************************************************************
* Global functions
******************************************************************************/
extern void FM_Variables_Init(void);

/******************************************************************************
* Inline functions
******************************************************************************/
inline void M1_PWM_UPDATE(GMCLIB_3COOR_T_F16 *psDuty)
{
	Word16 w16ModuloHalf = M1_MODULO_HALF;
	Word16 w16Result;
	
	/* PWM channels 0&1  */ 
	w16Result = mult_r(psDuty->f16A, w16ModuloHalf);

	setReg16(PWMA_SM0VAL2, -w16Result);
	setReg16(PWMA_SM0VAL3, w16Result);

	/* PWM channels 2&3 */								 
	w16Result = mult_r(psDuty->f16B, w16ModuloHalf);
	
	setReg16(PWMA_SM1VAL2, -w16Result);
	setReg16(PWMA_SM1VAL3, w16Result);
														 
	/* PWM channels 4&5 */								 
	w16Result = mult_r(psDuty->f16C, w16ModuloHalf);

	setReg16(PWMA_SM2VAL2, -w16Result);
	setReg16(PWMA_SM2VAL3, w16Result);
														 
	/* load new values to PWM registers */
	setReg16Bits(PWMA_MCTRL, PWMA_MCTRL_LDOK0_MASK | PWMA_MCTRL_LDOK1_MASK | PWMA_MCTRL_LDOK2_MASK); /* LDOK */ 
}

inline void M2_PWM_UPDATE(GMCLIB_3COOR_T_F16 *psDuty)
{
	Word16 w16ModuloHalf = M2_MODULO_HALF;
	Word16 w16Result;
	
	/* PWM channels 0&1  */ 
	w16Result = mult_r(psDuty->f16A, w16ModuloHalf);

	setReg16(PWMB_SM0VAL2, -w16Result);
	setReg16(PWMB_SM0VAL3, w16Result);

	/* PWM channels 2&3 */								 
	w16Result = mult_r(psDuty->f16B, w16ModuloHalf);

	setReg16(PWMB_SM1VAL2, -w16Result);
	setReg16(PWMB_SM1VAL3, w16Result);
														 
	/* PWM channels 4&5 */								 
	w16Result = mult_r(psDuty->f16C, w16ModuloHalf);

	setReg16(PWMB_SM2VAL2, -w16Result); // PWMB_SM3
	setReg16(PWMB_SM2VAL3, w16Result);
														 
	/* load new values to PWM registers */
	setReg16Bits(PWMB_MCTRL, PWMB_MCTRL_LDOK0_MASK | PWMB_MCTRL_LDOK1_MASK | PWMB_MCTRL_LDOK2_MASK); /* LDOK */ 			 
}

#endif /* _MCDEF_H_ */
