Code

32 bit Proportional-Integral (PI) Regulator

November 18, 20101 comment Coded in Mixed C and ASM for the Freescale DSP56F8xx

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 ------------- */