/*
 * dual_servo.h
 *
 *  Created on: May 18, 2015
 *      Author: B46639
 */

#ifndef DUAL_SERVO_H_
#define DUAL_SERVO_H_

#include "PE_types.h"
#include "intrinsics_56800E.h"
#include "intrinsics_56800EX.h"

typedef struct
{
	Word16 							w16SpeedCoeff;				/* Speed calculation coefficient */
	Word16							w16SpeedCalcScale;
	Word16							w16SpeedCalcScaleShift;
	Word16							w16EncCntRaw;				/* Raw captured encoder counter */
	Word16							w16EncCntRaw_1;
	Word16 							w16EncCnt;					/* Fractional Captured encoder position */
	Word16 							w16EncCnt_1;				/* Fractional Captured encoder position from previous step */
	Word16 							w16TCnt;					/* Captured reference timer value */
	Word16 							w16TCnt_1; 					/* Captured reference timer value from previous step */
	Word16							w16EncCntDelta;				/* EncCnt - EncCnt_1 */
	Word16							w16TCntDelta;				/* TCnt - TCnt_1 */
	UWord16							uw16TCntDeltaNow;			/* TCnt of current step */
	UWord16							uw16TCntDeltaLast;			/* TCnt of last step */
	Word32							w32TmpNumerator;			/* numerator = (w16SpeedCoeff * w16EncCntDelta) */
	Word16 							w16TimeOutFlag;				/* time out flag indicating motor is stopped */
	Frac32							f32Speed;
	Frac16							f16PositionMechScale;
	Word16							w16PositionMechScaleShift;
} MCSTRUC_SPEED_CALC_T;

typedef struct
{
	Frac16 f16SpdErr;				// speed error between speed reference and speed feedback
	Frac16 f16BaseTransform;		// coefficient for base transform
	Frac16 f16Prop;					// proportion coefficient of controller
	Frac16 f16Integ; 				// integral coefficient of controller
	Frac16 f16Kc; 					// the integral correction coefficient of controller 
	Frac16 f16LowSpeedProp;			// proportion coefficient when low speed
	Frac16 f16HighSpeedProp;		// proportion coefficient when high speed
	Frac16 f16LowSpeedInteg;		// integral coefficient when low speed
	Frac16 f16HighSpeedInteg;		// integral coefficient when high speed
	Frac16 f16PropScope;			// the scope proportion coefficient between low speed and high speed
	Frac16 f16IntegScope;			// the scope integral coefficient between low speed and high speed
	Frac32 f32LowSpeed;				// low speed (RPM)
	Frac32 f32HighSpeed;			// high speed (RPM)
	Frac32 f32ReciprlSpeedScope;	// reciprocal of the scope between high speed and low speed
	Frac16 f16Percentage;			// the Percentage: speed reference sub low speed, then divide the speed scope
	Frac32 f32PropAcc;				// proportional part of the calculation
	Frac32 f32IntegAcc;				// integral part of the calculation
	Frac32 f32KcPart;				// integral correction part of the calculation
	Frac32 f32SatErr;				// Saturated difference
	Word16 w16SatFlag;				// flag indicate saturation
	Frac32 f32UpperLimit;			// upper limit value
	Frac32 f32LowerLimit;			// lower limit value
	Frac32 f32ResultPreSat;			// the calculation result before saturation 
	Frac32 f32ResultAftSat;			// the calculation result after saturation 
	Frac32 f32IqRef;				// output: q axis current reference
	Frac16 f16IqRef;				// output: q axis current reference	
	Word32 w32SpdBase;
	Word32 w32KpBase;
	
} MCSTRUC_SPEED_ANTI_WINDUP_PI_T;

typedef struct
{
	Frac32 f32X;						// input value
	Frac32 f32X_1;						// last time input value
	Frac32 f32Y; 						// output value
	Frac32 f32Y_1;						// last time output value
	Frac32 f32B1;						// b1 coefficient of the filter
	Frac32 f32B2;						// b2 coefficient of the filter
	Frac32 f32A2;						// a2 coefficient of the filter
	
} IIR32_FILTER_T;

typedef struct
{
	Frac32 f32StateVar0;				// State variable of the Jerk Filter 
	Frac32 f32StateVar1;  		
	Frac32 f32StateVar2;				
	Frac32 f32Gain;						// Gain coefficient: HZ
	
} SMOOTH_FILTER_STRUCT;

typedef struct
{
	Frac32 							f32PropGain;			// PropGain of the Position controller
	Frac16 							f16BaseTransform;		// coefficient for base transform
	Word32							w32PropBase;
	Word16 							w16ProPGainShift;  		// ProPGain Shift of the Position controller 
	Word16 							w16SatFlag;				// saturation flag of calculation
	Frac32 							f32UpperLimit;			// upper limit value
	Frac32 							f32LowerLimit;			// lower limit value
	Frac32 							f32ResultPreSat;		// result of calculation before saturation
	Frac32 							f32ResultAftSat;		// result of calculation after saturation
	Frac32 							f32SpeedRef;			// output of Position controller
	Frac32 							f32PropGainLow;			// proportion coefficient when low position error
	Frac32 							f32PropGainHigh;		// proportion coefficient when high position error
	Frac32 							f32PropScope;			// the scope proportion coefficient between high and low position error
	Frac32 							f32PosErr;				// position error (rounds)
	Frac32 							f32PosErrLow;			// low position error (Rounds)
	Frac32 							f32PosErrHigh;			// high position error (Rounds)
	Frac32 							f32ReciprlPosErrScope;	// reciprocal of the scope between high and low position error
	Frac32 							f32Percentage;			// the Percentage: position error sub low position error, then divide the position error scope
} MCSTRUC_POS_CONTROLLER_T;

typedef struct
{
//	GFLIB_RAMP16_T	RampI;
	Word16 			w16FlagState;				// flag indicating that which is the present state 
	Word16 			w16FlagOutput;				
	Word16 			w16FlagMax;					// flag indicating that current is maximum 
	Word16 			w16FlagSearchFinish;
	Frac16 			f16Angle_B;					// middle angle between angle2 and angle3
	Word32 			w32Angle_B;
	Frac16 			f16Angle_A;					// positive angle edge
	Word32 			w32Angle_A;
	Frac16			f16Angle_C;					// negative angle edge
	Word32 			w32Angle_C;
	Frac16 			f16AngleScope_AtoB;
	Word32 			w32AngleScope_AtoB;
	Frac16 			f16AngleScope;				// scope value between angle2 and angle3
	Word32 			w32AngleScope;
	Frac16 			f16AngleMixScope;			// the minimum angle range
	Frac16 			f16AngleOffset;				// rotor's angle offset
	Word32 			w32AngleOffset;
	Frac16 			f16AngleOutput;				// output: D axis current value
	Word32 			w32AngleOutput;
	Frac16 			f16ThetaInitFinish;			// result: initial rotor's position
	Frac16 			f16IdMax;					// maximum value of d axis current 
	Frac16 			f16IdMix;					// minimum value of d axis current
	Frac16 			f16IdIncr;					// increment of d axis current
	Frac16 			f16IdPreSet;				// preset value 
	Frac16 			f16IdStart;					// start value of every algorithm period
	Frac16 			f16IdMoved;					// how much current can move the rotor
	Frac16 			f16IdOutput;				// output: d axis current
	Frac16 			f16IdRamp;
	Word16 			w16ShockCnt;				// counter: algorithm running times  
	Word16 			w16ShockAmount;				// amount: algorithm running times
	Word16 			w16WaitCnt;					// counter: waiting times
	Word16 			w16WaitAmount;				// amount: waiting times
	Word16 			w16SpaceCnt;				// counter: waiting for rotor standstill
	Word16 			w16SpaceAmount;				// amount: waiting for rotor standstill
	Word16 			w16CounterPulseFilter;			
	Word16 			w16AmountPulseFilter;				
	Word16 			w16EncVal1;					// record the encoder value before enter into state1
	Word16 			w16EncVal2;					// record the encoder value before enter into state2
	Word16 			w16EncoderNow;				// the present value of encoder
	Word16 			w16EncoderBias1;				// bias between the present value and EncoderInit1 when state1
	Word16 			w16EncoderBias2;				// bias between the present value and EncoderInit3 when state2
	Word16 			w16EncoderOffset;			// encoder offset
	Word16 			w16EncoderMixErr;			// when encoder offset reach to EncoderMixErr,  it can be consider that rotor is moved.
	UWord16 		uw16EncoderOutput;
	Word16			w16PolePairs;
	Frac16			f16EncoderScale;
	Word16			w16EncoderScaleShift;
	Frac16			f16EncoderModuleHalf;
	UWord16			uw16FlagWrite;
} MCSTRUC_SEARCH_ROTOR_POS_T;

extern void 	MCSTRUC_SpeedClacMTMethod(MCSTRUC_SPEED_CALC_T *psPosition);
extern Frac32 	MCSTRUC_SpeedClacImproved(MCSTRUC_SPEED_CALC_T *ptr, Frac32 f32SpeedRaw);
extern void 	MCSTRUC_SpdCtrlAntiWindupParamLinear(MCSTRUC_SPEED_ANTI_WINDUP_PI_T *ptr, Frac32 f32SpeedFbk);
extern Frac16 	MCSTRUC_SpdCtrlAntiWindupCalc(MCSTRUC_SPEED_ANTI_WINDUP_PI_T *ptr, Frac32 f32SpdErr);
extern void 	IIR32FilterClear(IIR32_FILTER_T *ptr);
extern Frac32 	IIR32FilterCalc(IIR32_FILTER_T *ptr, Frac32 f32Input);
extern void 	SmoothFilterInit(SMOOTH_FILTER_STRUCT *ptr);
extern Frac32 	SmoothFilterCalc(Frac32 f32Input, SMOOTH_FILTER_STRUCT *ptr);
extern void 	SearchRotorPos(MCSTRUC_SEARCH_ROTOR_POS_T *ptr);

/******************************************************************************
* Inline functions
******************************************************************************/
/*=======================================================================================
 *
 * 	Return 32-bit value = l_numerator/(2*s_denominator), this is an integer division.
 * 	The denominator should be positive.	
 * 
 =======================================================================================*/
inline Word32 Div_int_ls_new(Word16 register s_denominator, Word32 register l_numerator) 
{
    
    Word32 register w32Numerator,w32Rslt;
    
    asm(.optimize_iasm on);
    w32Numerator = (l_numerator>>16);
  	
	// MSB(16bits)/denominator		 
  	asm(bfclr 	#$0001,SR);	// clear carry bit
	asm(rep 16);
	asm(div 	s_denominator,w32Numerator); // w32Numerator = (remainder:MSB quotient)
	
	w32Rslt = (w32Numerator<<16);		// MSB quotient --> MSB portion of w32Rslt

  	asm(bcs 	CARRYSET);	// jump to CARRYSET if the LSB of the quotient is one,which means carry bit is set 
  	asm(add 	s_denominator,w32Numerator);		// restore remainder
  	
  	// LSB(16bits)/denominator
  	w32Numerator &= 0xffff0000;
  	w32Numerator |= (l_numerator&0x0000ffff);
  	asm(bfclr 	#$0001,SR);	// clear carry bit
  	asm(rep 	16);
  	asm(div 	s_denominator,w32Numerator);
  	
  	w32Rslt |= (w32Numerator&0x0000ffff);
  	
  	return w32Rslt;
  	
CARRYSET:  	
  	// LSB(16bits)/denominator
  	w32Numerator &= 0xffff0000;
  	w32Numerator |= (l_numerator&0x0000ffff);
  	asm(bfclr 	#$0001,SR);	// clear carry bit
  	asm(rep 	16);
  	asm(div 	s_denominator,w32Numerator);
  	w32Numerator += 0x8000;
  	w32Rslt |= (w32Numerator&0x0000ffff);
  	
  	asm(.optimize_iasm off);
  	return w32Rslt;
}

inline Word32 Div_int_ll(Word32 register l_numerator, Word32 register l_denominator)
{
	Word32 register w32ClbDenom;
	Word16 register w16LeadBits; 
	// A is l_numerator, B is l_denominator
	
	asm(.optimize_iasm on);
	asm( tfr l_numerator, A);
	asm( tfr l_denominator, B);
	
	asm( tfr B, Y);				// w32ClbDenom is for normalized denominator
	asm( clb Y, w16LeadBits);		// count leading bits of denominator
	asm( asll.l w16LeadBits, Y);  	// normalize the denominator
	asm( sub Y, A);		// l_numerator - w32ClbDenom
	asm( bftstl #$8, A2); // set Carry if positive
	// divide loop
	asm( rep w16LeadBits);
	asm( div Y, A);
	asm( rol.l A);	// now low "w16LeadBits+1" bits of A contains the quotient
	// mask
	asm( eor.l Y, Y); // clear w32ClbDenom
	asm( add.l #2, Y);
	asm( asll.l w16LeadBits,Y);
	asm( dec.l Y); // w32ClbDenom contains the mask
	asm( and.l Y, A); // apply the mask
	asm( tfr A, l_numerator);
	asm(.optimize_iasm off);
	return l_numerator;
}


#endif /* DUAL_SERVO_H_ */
