/******************************************************************************
*
* Freescale Semiconductor Inc.
* (c) Copyright 2012 Freescale Semiconductor, Inc.
* ALL RIGHTS RESERVED.
*
***************************************************************************//*!
*
* @file     bemf_Observer.c
*
* @author   b35258
*
* @version  0.0.1.0
*
* @date     Apr-27-2015
*
* @brief    Bemf_Observer - Observer of Rotor position in d/q reference frame
*
*******************************************************************************
*
* This file includes initial setting and MACRO definitions of Bemf Observer of 
* Rotor position in d/q reference frame
*
******************************************************************************/

#include "bemf_Observer.h"

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



/******************************************************************************
* MACRO definitions
******************************************************************************/


/******************************************************************************
* Functions Definitions
*******************************************************************************/
volatile static SWLIBS_2Syst_F16 Igade, Ugade;
void ACLIB_PMSMBemfObsrvDQ(tFrac16 *f16SpeedEst, tFrac16 *PosEst, SWLIBS_2Syst_F16 *pudtCurrentAlbe, SWLIBS_2Syst_F16 *pudtVoltageAlbe, ACLIB_BEMF_OBSRV_DQ_T *pudtCtrl)
{
	SWLIBS_2Syst_F16 EstimGaDeframe;


	tFrac16 alphaNegDivBeta;

	// Transformacie U,I -> dq_estsustava
	// Electrical rotor positio
	EstimGaDeframe.f16Arg1   = GFLIB_Sin(*PosEst); // !_!
	EstimGaDeframe.f16Arg2   = GFLIB_Cos(*PosEst); // !_!

	// Transform to Gama Delta frame
	GMCLIB_Park_F16((SWLIBS_2Syst_F16 *)&Igade, &EstimGaDeframe, pudtCurrentAlbe);
	GMCLIB_Park_F16((SWLIBS_2Syst_F16 *)&Ugade, &EstimGaDeframe, pudtVoltageAlbe);

	// PI controllers for Back EMF estimation
	pudtCtrl->udtEObsrv.f16Arg1 = GFLIB_ControllerPIrAW_F16(MLIB_SubSat_F16(Igade.f16Arg1, pudtCtrl->udtIObsrv.f16Arg1), &pudtCtrl->PI_Ga_Emf);
	pudtCtrl->udtEObsrv.f16Arg2 = GFLIB_ControllerPIrAW_F16(MLIB_SubSat_F16(Igade.f16Arg2, pudtCtrl->udtIObsrv.f16Arg2), &pudtCtrl->PI_De_Emf);

	// riadeny system, in: Usum, out: Idq_est
	// id_frac_est = (ud_frac*K1_n + w_n*iq_frac*K2_n + ed_frac*K3_n + id_frac_est*K4_n);
	// iq_frac_est = (uq_frac*K1_n - w_n*id_frac*K2_n + eq_frac*K3_n + iq_frac_est*K4_n);
	pudtCtrl->udtIObsrv.f16Arg1 	= (tFrac16)((MLIB_Add_F32(MLIB_Add_F32(MLIB_Mul_F32F16F16(Ugade.f16Arg1,pudtCtrl->K1n),
														                   MLIB_Mul_F32F16F16((tFrac16)(MLIB_Mul_F32F16F16(*f16SpeedEst,Igade.f16Arg2) >> 16),
														    		  	  	  	               pudtCtrl->K2n
														    		  	  	  	              )
														                  ),
														      MLIB_Add_F32(MLIB_Mul_F32F16F16(pudtCtrl->udtEObsrv.f16Arg1,pudtCtrl->K3n),
														    		  	   MLIB_Mul_F32F16F16(pudtCtrl->udtIObsrv.f16Arg1,pudtCtrl->K4n)
														      	  	  	  )
															)
											    ) >> (16 - pudtCtrl->Kscale_n));


	pudtCtrl->udtIObsrv.f16Arg2 	= (tFrac16)((MLIB_Add_F32(MLIB_Sub_F32(MLIB_Mul_F32F16F16(Ugade.f16Arg2,pudtCtrl->K1n),
			                                                               MLIB_Mul_F32F16F16((tFrac16)(MLIB_Mul_F32F16F16(*f16SpeedEst,Igade.f16Arg1) >> 16),
			                                                            		               pudtCtrl->K2n
			                                                            		             )
			                                                              ),
			                                                  MLIB_Add_F32(MLIB_Mul_F32F16F16(pudtCtrl->udtEObsrv.f16Arg2,pudtCtrl->K3n),
			                                                		       MLIB_Mul_F32F16F16(pudtCtrl->udtIObsrv.f16Arg2,pudtCtrl->K4n)
			                                                		      )
			                                                 )
			                                    ) >> (16 - pudtCtrl->Kscale_n));

	// calculate position error between DQ and GamaDelta coordination frames
	alphaNegDivBeta = MLIB_DivSat_F16(MLIB_Mul_F16(pudtCtrl->udtEObsrv.f16Arg1,FRAC16(-1.0)),
									  pudtCtrl->udtEObsrv.f16Arg2
									 );

	pudtCtrl->DQtoGaDeError = GFLIB_Atan(alphaNegDivBeta);

	// minimize the position error between DQ and GamaDelta coordination frames
	ACLIB_TrackObsrv(f16SpeedEst, PosEst, pudtCtrl->DQtoGaDeError, &pudtCtrl->TrackObsrv);
}


/******************************************************************************
* Functions Definitions
*******************************************************************************/
void ACLIB_TrackObsrv(tFrac16 *f16SpeedEst, tFrac16 *PosEst, tFrac16 DQtoGaDeError, ACLIB_TRACK_OBSRV_T *pudtCtrl)
{
	// Tracking observer
	// PI controller of TO
	*f16SpeedEst = GFLIB_ControllerPIrAW(DQtoGaDeError, &pudtCtrl->PI_Pospe_Obs_Rec);
	// Speed filtering - 1. order
	*f16SpeedEst = GDFLIB_FilterMA_F16(*f16SpeedEst,&pudtCtrl->filterMA);
	// Integrator of TO
	*PosEst = GFLIB_IntegratorTR_F16(*f16SpeedEst,&pudtCtrl->Ki_IntAto);
}




