/****************************************************************************************
*										       *
*  Library: PI control					       *

* File: pi.dsp								       *
*										       *
* Description:  PI control code file						       *
* Purpose    : Library routines for PI controller									       *
* 										       *
* Author     : JC								       *
* Version    : 1.0								       *
* Date       : Dec 2001                                                            *
* Modification History:    none							       *
*										       *
* Embedded Control Systems							       *
* Analog Devices Inc.								       *
****************************************************************************************/

#include <main.h>;

/***************************************************************************************
* Calculate Configuration Register Contents from Parameters                            *
***************************************************************************************/
//None

/***************************************************************************************
* Constants Defined in the Module                                                      *
***************************************************************************************/

/*none*/

/***************************************************************************************
* Routines Defined in this Module                                                      *
***************************************************************************************/

.global  PI32_Ctrl_;

/***************************************************************************************
* Global Variables Defined in this Module                                              *
***************************************************************************************/

/* None */

/***************************************************************************************
* Local Variables Defined in this Module                                               *
***************************************************************************************/

/* None*/ 

/***************************************************************************************
*                                                                                      *
* Type: Routine                                                       		       *
*                                                                                      *
* Call: call PI32_Ctrl_;                                                            *
*                                                                                      *
* Implements a single precision PI controller                                          *
*                                                                                      *
* Inputs  : I3: pointer to Delay line buffer                                       *
* 	        M3: 1								       *
* 	        L3: length of Delay line buffer                                        *
* 	  		I7: pointer to Coefficient buffer                                      *
* 	        M7: 1								       *
* 	        L7: length of Coefficient buffer                                       *
* 	        ar: Input error value in 1.15 format				       *
* 			ay0:Scale Factor = number of shifts to the left in the barrel shifter  *
*                                                                                      *
* Outputs  :	sr1: Output value in 1.15 format                                       *
*                                                                                      *
* Modified:	I3, L3, ax0, B3, M3, I7, L7, ay0, mx1, mx0, mr1, mr0, ar, se, sr, my0	   *
*                                                                                      *
***************************************************************************************/
.section/pm seg_pmcode;

PI32_Ctrl_:				 


	mx1=ar;						// I(k+1)		
	mx0=DM(I3+=M3);  			//	 I(k)			

	mr1=DM(I3+=M3);				// U(k)_MSW		
	mr0=DM(I3+=M3);	        	// U(k)_LSW		
	ar=-ay0;
	SE=ar;
	sr = ASHIFT mr1 (HI);
	sr = sr OR LSHIFT mr0 (LO);
	mr1=sr1;					// B0*U(k)_MSW		
	mr0=sr0;					// B0*U(k)_LSW		
	
 	my0 = PM(I7+=M7);			// A0	
	mr = mr+mx0*my0 (SS),   my0 = PM(I7+=M7);	//+A0*I(k),        A1	
	mr = mr+mx1*my0 (SS),	DM(I3+=M3)= mx1;	//+A1*I(k+1), I(k)=I(k+1)
	sat mr;

	SE=EXP mr1 (HI);
	ax0=SE;
	ar=ax0+ay0;
	if gt jump PISaturate32;

	SE=ay0;
	sr = ASHIFT mr1 (HI);
	sr = sr OR LSHIFT mr0 (LO);
	DM(I3,M3) = sr1;				// U(k)=U(k+1) MSW	
	DM(I3,M3) = sr0;				// U(k)=U(k+1) LSW	
	rts;

PISaturate32:
	astat=0x40;
	sat mr;
	DM(I3+=M3) = mr1;				// U(k)=sat(U(k+1)) MSW	
	DM(I3+=M3) = mr0;				// U(k)=sat(U(k+1)) LSW	
	sr1=mr1;
	rts;