32 bit Proportional-Integral (PI) Regulator
This library implements a C-callable ASM PI regulator for the Freescale DSP56F800E family of digital signal controllers.
The proportional gain is described as kp*2^nkp, where kp is the 1.15 fractional part and nkp is the binary exponent; this allows proportional gains greater than 1 in a 1.15 environment. The proportional output is correctly saturated 1.15.
The integral memory is 32-bit wide and can be saturated to reduce wind-up. A separate saturation can be enforced on the whole output as well.
Source includes the header file for inclusion in C applications as well as the code itself and an usage example in C.
/* -------------- PI32.h begins -------------- */
#ifndef __PI32_H__
#define __PI32_H__
#include "types.h"
typedef struct
{
Frac16 c0;
Frac16 c1;
Frac16 IntSatH;
Frac16 IntSatL;
Frac16 kp;
UInt16 nkp;
Frac16 OutSatH;
Frac16 OutSatL;
}PI32params;
typedef struct
{
Frac32 Skm1;
Frac16 xkm1;
}PI32data;
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data);
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data);
#endif //ifndef __PI32_H__
/* -------------- PI32.h ends -------------- */
/* -------------- PI32.c begins -------------- */
/*---------------------------------------------------------------------------;
; Initialize integral memory and past input of PI regulator. ;
; ;
; Input: A10 = Integral memory initialization value ;
; Y0 = Past input initialization value ;
; (R2) = Integral memory storage destination ;
; (R2+2) = Past input storage destination ;
; ;
; Output: None ;
; ;
; Registers modified: R2 ;
;---------------------------------------------------------------------------*/
asm void PI32Init(Frac32 S, Frac16 xp, PI32data *data){
MOVE.L A10,X:(R2)+ //Initialize integral memory
MOVE.W Y0,X:(R2) //Initialize past input
RTS //Return from subroutine
}
/*---------------------------------------------------------------------------;
; This function implements a 32 bit precision PI regulator with separate ;
; saturations on the integral portion and on the whole output. ;
; The routine first computes the integral portion at current sampling ;
; instant k (Sk) as Sk = Sk-1 + c0*xk + c1*xk-1. Sk is subsequently ;
; saturated between IntSatH and IntSatL and saved for later use at the next ;
; sampling instant. ;
; Next, proportional portion Pk is computed as kp*2^nkp*xk and saturated ;
; between -1 and +1. The two portions are added together and the result ;
; yk=Sk+Pk is saturated between OutSatH and OutSatL. ;
; Coefficients c0 and c1 are equal and amount to ki*Ts/2, where Ts is the ;
; sampling period and ki is the integral multiplier. The proportional ;
; multiplier kp*2^nkp is split into a fractional part and an exponent in ;
; order to allow values greater than unity. ;
; For optimal results it is advised to call this function with saturation ;
; disabled. ;
; ;
; Input: Y0 = xk (input sample of error signal) ;
; (R2) = c0 ;
; (R2+1) = c1 ;
; (R2+2) = IntSatH ;
; (R2+3) = IntSatL ;
; (R2+4) = kp ;
; (R2+5) = nkp ;
; (R2+6) = OutSatH ;
; (R2+7) = OutSatL ;
; (R3) = Sk-1 (32 bits) ;
; (R3+2) = xk-1 ;
; ;
; Output: A = yk (32 significant bits) ;
; ;
; Registers modified: A, B, X0, Y, R0 ;
;---------------------------------------------------------------------------*/
asm Frac32 PI32(Frac16 xk, PI32params *params, PI32data *data){
MOVE.W R2,X0
MOVE.W X0,R0
MOVE.L X:(R3)+,A //Load Sk-1
MOVE.W X:(R0)+,X0 //Load c0
MOVE.W Y0,Y1 //Save xk
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //c0*xk, c1, xk-1
MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //c1*xk-1, IntSatH, decr. R3
MOVE.W Y0,B //Load IntSatH into B
CMP A,B Y1,X:(R3)+ //Check upper satur. and save xk
BGT CheckSatLo //If Sk<IntSatH no need for sat.
MOVE.W Y0,A //else saturate and increment R0
ADDA #1,R0 //to skip IntSatL in the
BRA IntSatOK //parameter structure
CheckSatLo: MOVE.W X:(R0)+,B //Load IntSatL
CMP A,B //Check lower saturation
BLE IntSatOK //If Sk>IntSatL no need for sat.
MOVE.W B1,A //else saturate
IntSatOK: ADDA #-3,R3,R3 //Point to Sk storage location
MOVE.L A10,X:(R3) //Save Sk
MOVE.W X:(R0)+,Y0 //Load kp
MPY Y0,Y1,B X:(R0)+,Y0 //kp*xk, nkp
CLB B,X0 //Number of leading bits of
CMP Y0,X0 //result greater or equal than
NOP //desired shift count?
BLT PropSat //No, saturate
ASLL.L Y0,B //Yes, shift
BRA PropSatOK
PropSat: TST B //Saturate proportional portion
BLT PropSatNeg //according to sign
MOVE.W #$7FFF,B
BRA PropSatOK
PropSatNeg: MOVE.W #$8000,B
PropSatOK: ADD B,A X:(R0)+,B //Compute yk, load OutSatH
CMP A,B X:(R0)+,X0 //Check upper sat., load OutSatL
BGT CheckSatOutLo //If yk<OutSatH no need for sat.
MOVE.W B1,A //else saturate
BRA OutSatOK
CheckSatOutLo: CMP X0,A //Check lower saturation
BGT OutSatOK //If yk>OutSatL no need for sat.
MOVE.W X0,A //else saturate
OutSatOK: RTS //Return from subroutine
}
/* -------------- PI32.c ends -------------- */
/* -------------- Usage example begins ------------- */
/* application specific constants */
#define PIc0 100
#define PIc1 100
#define PIIntSatH 15000
#define PIIntSatL -15000
#define PIkp 0x4000
#define PInkp 1
#define PIOutSatH 30000
#define PIOutSatL -30000
/* application specific includes */
#include "PI32.h"
/* global variables */
PI32params PIparams;
PI32data PIdata;
int PIIn,PIOut;
/* initializations */
PIparams.c0=PIc0; //Initialize PI regulator
PIparams.c1=PIc1; //parameters and past data
PIparams.IntSatH=PIIntSatH;
PIparams.IntSatL=PIIntSatL;
PIparams.kp=PIkp;
PIparams.nkp=PInkp;
PIparams.OutSatH=PIOutSatH;
PIparams.OutSatL=PIOutSatL;
PI32Init(0,0, &PIdata);
/* PI32 regulator function call */
PIOut=(int)(PI32(PIIn,&PIparams,&PIdata)>>16); //Call PI regulator
/* -------------- Usage example ends ------------- */
