Code

Sine and cosine functions through MacLaurin expansion

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
/* -------------- SinCos.h begins -------------- */

#ifndef __SINCOS_H__
#define __SINCOS_H__

#include "types.h"

/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series     ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative    ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in   ;
; subsequent calculations.                                                   ;
;                                                                            ;
; Input:  Y0 = angle (signed fractional; -1 = -pi, +1 = +pi)                 ;
;                                                                            ;
; Output: Y0 = sine (signed fractional)                                      ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle);

/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series   ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative    ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in   ;
; subsequent calculations.                                                   ;
;                                                                            ;
; Input:  Y0 = angle (signed fractional; -1 = -pi, +1 = +pi)                 ;
;                                                                            ;
; Output: Y0 = cosine (signed fractional)                                    ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle);

#endif //ifndef __SINCOS_H__

/* -------------- SinCos.h ends -------------- */

/* -------------- SinCos.c begins -------------- */

/*---------------------------------------------------------------------------;
; This function computes the sine of an angle using the Maclaurin series     ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative    ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in   ;
; subsequent calculations.                                                   ;
;                                                                            ;
; Input:  Y0 = angle (signed fractional; -1 = -pi, +1 = +pi)                 ;
;                                                                            ;
; Output: Y0 = sine (signed fractional)                                      ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm Frac16 Sin(Frac16 angle){
                CLR.W Y1                     //Clear Y1
                MOVE.W Y0,B                  //Compute absolute value of angle
                ABS B
                CMP.W #$4000,B               //|angle| > pi/2?
                BLT SinAngleOK               //No, proceed
                ADD.W #$8000,Y0              //Yes, add pi and set flag for
                BFSET #$0001,Y1              //final negation of result 
SinAngleOK:     MPYR Y0,Y0,A                 //Compute angle squared
                MOVE.W A,X0                  //X0 = angle squared
                MOVE.W #$0002,B              //Compute Maclaurin expansion
                MOVE.W #$FFE2,A
                MACR X0,B1,A                 //A1 = first partial result
                MOVE.W #$0150,B
                MACR X0,A1,B                 //B1 = second partial result
                MOVE.W #$F669,A
                MACR X0,B1,A                 //A1 = third partial result
                MOVE.W #$28CD,B
                MACR X0,A1,B                 //B1 = fourth partial result
                MOVE.W #$AD52,A
                MACR X0,B1,A                 //A1 = fifth partial result
                MOVE.W #$3244,B
                MACR X0,A1,B                 //B1 = sixth partial result
                MOVE.W #$0003,X0             //Required shift amount
                MPYR B1,Y0,A                 //A = result / 8
                CMP #$1000,A                 //If magnitude is maximum
                BNE SinCheckSatL             //saturate the result
                MOVE.W #$7FFF,A
                BRA SinValOK
SinCheckSatL:   CMP.W #$F000,A
                BNE SinNoSat
                MOVE.W #$8001,A
                BRA SinValOK
SinNoSat:       ASLL.L X0,A                  //Shift to get correct result
                                             //(Maclaurin coefficients are
                                             //divided by 8 to avoid overflow)
SinValOK:       BRCLR #$0001,Y1,Update_Sin   //Negate result if |angle| was
                NEG A                        //greater than pi/2
Update_Sin:     MOVE.W A,Y0                  //Save result
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; This function computes the cosine of an angle using the Maclaurin series   ;
; expansion method. Input angle is signed fractional where -1 corresponds to ;
; -pi and +1 to +pi; output is signed fractional and the minimum negative    ;
; value is limited to -1+2^(-15) (0x8001 hexadeximal) to avoid overflow in   ;
; subsequent calculations.                                                   ;
;                                                                            ;
; Input:  Y0 = angle (signed fractional; -1 = -pi, +1 = +pi)                 ;
;                                                                            ;
; Output: Y0 = cosine (signed fractional)                                    ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm Frac16 Cos(Frac16 angle){
                ADD.W #$4000,Y0              //Add pi/2 to angle
                JSR Sin                      //Call sine function
                RTS                          //Return from subroutine
}

/* -------------- SinCos.c ends -------------- */

/* -------------- Usage example begins ------------- */

/* application specific includes */
#include "SinCos.h"

/* global variables */
Frac16 Angle,cosphi,sinphi;

/* Function calls */
  cosphi=Cos(Angle);                         //Compute sin and cos of angle
  sinphi=Sin(Angle);                         //(for use in Park transforms)

/* -------------- Usage example ends ------------- */

Clarke and Park direct and inverse transforms

November 18, 20101 comment Coded in Mixed C and ASM for the Freescale DSP56F8xx
/* -------------- CPTrans.h begins -------------- */

#ifndef __CPTRANS_H__
#define __CPTRANS_H__

#include "types.h"

#define Minus1Over2 0xC000
#define Root3Over2 0x6EDA
#define TwoOver3 0x5555
#define Minus1Over3 0xD555
#define OneOnRoot3 0x49E7

/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes.                   ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  (R2)   = Vu                                                        ;
;         (R2+1) = Vv                                                        ;
;         (R2+2) = Vw                                                        ;
;         (R3)   = Destination memory location for Va                        ;
;         (R3+1) = Destination memory location for Vb                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, X0, Y                                               ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab);

/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes.           ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  (R2)   = Va                                                        ;
;         (R2+1) = Vb                                                        ;
;         (R3)   = Destination memory location for Vu                        ;
;         (R3+1) = Destination memory location for Vv                        ;
;         (R3+2) = Destination memory location for Vw                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw);

/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq).                                               ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = cos(phi)                                                  ;
;         Y1     = sin(phi)                                                  ;
;         (R2)   = Va                                                        ;
;         (R2+1) = Vb                                                        ;
;         (R3)   = Destination memory location for Vd                        ;
;         (R3+1) = Destination memory location for Vq                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0                                               ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq);

/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab).                                       ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = cos(phi)                                                  ;
;         Y1     = sin(phi)                                                  ;
;         (R2)   = Vd                                                        ;
;         (R2+1) = Vq                                                        ;
;         (R3)   = Destination memory location for Va                        ;
;         (R3+1) = Destination memory location for Vb                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0                                               ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab);

#endif //ifndef __CPTRANS_H__

/* -------------- CPTrans.h ends -------------- */

/* -------------- CPTrans.c begins -------------- */

/*---------------------------------------------------------------------------;
; Clarke transform (Vuvw -> Vab) with constant amplitudes.                   ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  (R2)   = Vu                                                        ;
;         (R2+1) = Vv                                                        ;
;         (R2+2) = Vw                                                        ;
;         (R3)   = Destination memory location for Va                        ;
;         (R3+1) = Destination memory location for Vb                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, X0, Y                                               ;
;---------------------------------------------------------------------------*/
asm void Clarke(Frac16 *Vuvw, Frac16 *Vab){
                MOVE.W X:(R2),X0             //X0 = Vu
                MOVE.W #TwoOver3,Y0          //Y0 = 2/3
                MPY X0,Y0,A                  //A  = 2/3*Vu
                MOVE.W #Minus1Over3,X0       //X0 = -1/3
                MOVE.W X:(R2+1),Y0           //Y0 = Vv
                MOVE.W X:(R2+2),Y1           //Y1 = Vw
                MAC X0,Y0,A                  //A  = 2/3*Vu-1/3*Vv
                MAC X0,Y1,A                  //A  = 2/3*Vu-1/3*Vv-1/3*Vw
                MOVE.W A,X:(R3)              //Va = 2/3*Vu-1/3*Vv-1/3*Vw
                MOVE.W #OneOnRoot3,X0        //X0 = 1/sqrt(3)
                MPY X0,Y0,A                  //A  = 1/sqrt(3)*Vv
                MACR -X0,Y1,A                //A  = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
                MOVE.W A,X:(R3+1)            //Vb = 1/sqrt(3)*Vv-1/sqrt(3)*Vw
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; Inverse Clarke transform (Vab -> Vuvw) with constant amplitudes.           ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  (R2)   = Va                                                        ;
;         (R2+1) = Vb                                                        ;
;         (R3)   = Destination memory location for Vu                        ;
;         (R3+1) = Destination memory location for Vv                        ;
;         (R3+2) = Destination memory location for Vw                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0, Y                                            ;
;---------------------------------------------------------------------------*/
asm void InvClarke(Frac16 *Vab, Frac16 *Vuvw){
                MOVE.W X:(R2),Y0             //Y0 = Va
                MOVE.W X:(R2+1),Y1           //Y1 = Vb
                MOVE.W Y0,X:(R3)             //Vu = Va
                MOVE.W #Minus1Over2,X0       //X0 = -1/2
                MPY X0,Y0,A                  //A  = -1/2*Va
                TFR A,B                      //B  = -1/2*Va
                MOVE.W #Root3Over2,X0        //X0 = sqrt(3)/2
                MAC X0,Y1,A                  //A  = -1/2*Va+sqrt(3)/2*Vb
                MAC -X0,Y1,B                 //B  = -1/2*Va-sqrt(3)/2*Vb
                MOVE.W A,X:(R3+1)            //Vv = -1/2*Va+sqrt(3)/2*Vb
                MOVE.W B,X:(R3+2)            //Vw = -1/2*Va-sqrt(3)/2*Vb
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; Park transform (Vab -> Vdq).                                               ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = cos(phi)                                                  ;
;         Y1     = sin(phi)                                                  ;
;         (R2)   = Va                                                        ;
;         (R2+1) = Vb                                                        ;
;         (R3)   = Destination memory location for Vd                        ;
;         (R3+1) = Destination memory location for Vq                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0                                               ;
;---------------------------------------------------------------------------*/
asm void Park(Frac16 cosphi, Frac16 sinphi, Frac16 *Vab, Frac16 *Vdq){
                MOVE.W X:(R2),X0             //X0 = Va
                MPY X0,Y0,A                  //A  = Va*cos(phi) 
                MPY -X0,Y1,B                 //B  = -Va*sin(phi)
                MOVE.W X:(R2+1),X0           //X0 = Vb
                MAC X0,Y1,A                  //A  = Va*cos(phi)+Vb*sin(phi)
                MAC X0,Y0,B                  //B  = -Va*sin(phi)+Vb*cos(phi)
                MOVE.W A,X:(R3)              //Vd = Va*cos(phi)+Vb*sin(phi)
                MOVE.W B,X:(R3+1)            //Vq = -Va*sin(phi)+Vb*cos(phi)
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; Inverse Park transform (Vdq -> Vab).                                       ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = cos(phi)                                                  ;
;         Y1     = sin(phi)                                                  ;
;         (R2)   = Vd                                                        ;
;         (R2+1) = Vq                                                        ;
;         (R3)   = Destination memory location for Va                        ;
;         (R3+1) = Destination memory location for Vb                        ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: A, B, X0                                               ;
;---------------------------------------------------------------------------*/
asm void InvPark(Frac16 cosphi, Frac16 sinphi, Frac16 *Vdq, Frac16 *Vab){
                MOVE.W X:(R2),X0             //X0 = Vd
                MPY X0,Y0,A                  //A  = Vd*cos(phi) 
                MPY X0,Y1,B                  //B  = Vd*sin(phi)
                MOVE.W X:(R2+1),X0           //X0 = Vq
                MAC -X0,Y1,A                 //A  = Vd*cos(phi)-Vq*sin(phi)
                MAC X0,Y0,B                  //B  = Vd*sin(phi)+Vq*cos(phi)
                MOVE.W A,X:(R3)              //Vd = Vd*cos(phi)-Vq*sin(phi)
                MOVE.W B,X:(R3+1)            //Vq = Vd*sin(phi)+Vq*cos(phi)
                RTS                          //Return from subroutine
}

/* -------------- CPTrans.c ends -------------- */

/* -------------- Usage example begins ------------- */

/* application specific includes */
#include "CPTrans.h"

/* global variables */
Frac16 cosphi,sinphi;
Frac16 Vuvw[3];
Frac16 Vab[2];
Frac16 Vdq[2];
Frac16 Vab1[2];
Frac16 Vuvw1[3];

/* Transform function calls */
//This example performs the full processing chain from three-phase fixed to
//two-phase rotating reference and back. In the end Vuvw and Vuvw1 should be
//equal save for rounding errors.
  Clarke((Frac16*)&Vuvw,(Frac16*)&Vab);
  Park(cosphi,sinphi,(Frac16*)&Vab,(Frac16*)&Vdq);
  InvPark(cosphi,sinphi,(Frac16*)&Vdq,(Frac16*)&Vab1);
  InvClarke((Frac16*)&Vab1,(Frac16*)&Vuvw1);

/* -------------- Usage example ends ------------- */

Second order IIR filter with decimation

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
/* -------------- IIR2.h begins -------------- */

#ifndef __IIR2_H__
#define __IIR2_H__

#include "types.h"

typedef struct
{
  unsigned int decimation;
  Frac16 b0over2;
  Frac16 b1over2;
  Frac16 b2over2;
  Frac16 ma1over2;
  Frac16 ma2over2;
}IIR2params;

typedef struct
{
  unsigned int deccnt;
  Frac16 xkm1;
  Frac16 xkm2;
  Frac16 ykm1;
  Frac16 ykm2;
}IIR2data;

/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero.             ;
;                                                                            ;
; 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 IIR2Init(IIR2data *data);

/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter.             ;
; The result yk is computed as a function of current (xk) and past (xk-1,    ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows:                     ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2.                        ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude  ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2).      ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = xk (input sample)                                         ;
;         (R2)   = decimation (1 = no decimation, 2 = discard every other    ;
;                  sample, and so on)                                        ;
;         (R2+1) = b0/2                                                      ;
;         (R2+2) = b1/2                                                      ;
;         (R2+3) = b2/2                                                      ;
;         (R2+4) = -a1/2                                                     ;
;         (R2+5) = -a2/2                                                     ;
;         (R3)   = decimation counter (used internally for data decimation)  ;
;         (R3+1) = xk-1                                                      ;
;         (R3+2) = xk-2                                                      ;
;         (R3+3) = yk-1                                                      ;
;         (R3+4) = yk-2                                                      ;
;                                                                            ;
; Output: Y0     = yk                                                        ;
;                                                                            ;
; Registers modified: A, X0, Y, R0, R3                                       ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data);

#endif //ifndef __IIR2_H__

/* -------------- IIR2.h ends -------------- */

/* -------------- IIR2.c begins -------------- */

/*---------------------------------------------------------------------------;
; Initialize decimation divider and past data of filter to zero.             ;
;                                                                            ;
; 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 IIR2Init(IIR2data *data){
                MOVE.W #0001,X:(R2)          //Initialize decimation divider
                ADDA #1,R2                   //to execute the first call
                REP #4                       //Initialize past data to zero
                CLR.W X:(R2)+
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; This function implements a second order IIR Butteworth filter.             ;
; The result yk is computed as a function of current (xk) and past (xk-1,    ;
; xk-2) inputs and past (yk-1, yk-2) outputs as follows:                     ;
; yk = b0*xk + b1*xk-1 + b2*xk-2 - a1*yk-1 - a2*yk-2.                        ;
; Filter coefficients bi and aj are stored halved to ensure their magnitude  ;
; does not exceed 1 (coefficients magnitude can be at most equal to 2).      ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = xk (input sample)                                         ;
;         (R2)   = decimation (1 = no decimation, 2 = discard every other    ;
;                  sample, and so on)                                        ;
;         (R2+1) = b0/2                                                      ;
;         (R2+2) = b1/2                                                      ;
;         (R2+3) = b2/2                                                      ;
;         (R2+4) = -a1/2                                                     ;
;         (R2+5) = -a2/2                                                     ;
;         (R3)   = decimation counter (used internally for data decimation)  ;
;         (R3+1) = xk-1                                                      ;
;         (R3+2) = xk-2                                                      ;
;         (R3+3) = yk-1                                                      ;
;         (R3+4) = yk-2                                                      ;
;                                                                            ;
; Output: Y0     = yk                                                        ;
;                                                                            ;
; Registers modified: A, X0, Y, R0, R3                                       ;
;---------------------------------------------------------------------------*/
asm Frac16 IIR2(Frac16 xk, IIR2params *params, IIR2data *data){
                DEC.W X:(R3)                 //Decrement decimation counter
                BEQ DoFilter                 //Decremented to zero?
                MOVE.W X:(R3+3),Y0           //No, output old value and exit
                BRA FilterDone
DoFilter:       MOVE.W X:(R2),X0             //Yes, reload decimation counter
                MOVE.W X0,X:(R3)+
                ADDA #1,R2,R0                //Use R0 for parallel moves
                MOVE.W Y0,Y1                 //Save xk
                MOVE.W X:(R0)+,X0            //b0/2
                MPY Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b0/2*xk, b1/2, xk-1
                MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b1/2*xk-1, b2/2, xk-2
                MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //b2/2*xk-2, -a2/2, yk-1
                MAC Y0,X0,A X:(R0)+,Y0 X:(R3)-,X0 //-a2/2*yk-1, -a3/2, yk-2
                MAC Y0,X0,A X:(R0)+,Y0 X:(R3)+,X0 //-a3/2*yk-2, n/a, yk-1
                ASL A                        //Compensate for halved coeffs
                MOVE.W A,Y0                  //Saturate result if necessary
                MOVE.W X0,X:(R3)-            //yk-2=yk-1
                MOVE.W Y0,X:(R3)-            //yk-1=yk
                ADDA #-1,R3                  //Point to xk-1
                MOVE.W X:(R3)+,X0            //xk-1
                MOVE.W X0,X:(R3)-            //xk-2=xk-1
                MOVE.W Y1,X:(R3)+            //xk-1=xk
FilterDone:     RTS                          //Return from subroutine
}

/* -------------- IIR2.c ends -------------- */

/* -------------- Usage example begins ------------- */

/* application specific constants */
#define IIRDec 160                           //IIR filter decimation factor
#define IIRb0 0x0019                         //IIR filter parameters: 128 Hz
#define IIRb1 0x0032                         //lowpass (with 10 kHz sampling
#define IIRb2 0x0019                         //frequency)
#define IIRa1 0x78BA
#define IIRa2 0xC6E1

/* application specific includes */
#include "IIR2.h"

/* global variables */
IIR2params IIRparams;
IIR2data IIRdata;
int FiltIn,IIROut;

/* initializations */
  IIRparams.decimation=IIRDec;               //Initialize filter decimation
  IIRparams.b0over2=IIRb0;                   //Initialize filter parameters
  IIRparams.b1over2=IIRb1;
  IIRparams.b2over2=IIRb2;
  IIRparams.ma1over2=IIRa1;
  IIRparams.ma2over2=IIRa2;
  IIR2Init(&IIRdata);                        //Initialize IIR filter past data

/* IIR filter function call */
  IIROut=IIR2(FiltIn,&IIRparams,&IIRdata);   //Call IIR filter

/* -------------- Usage example ends ------------- */

Moving average filter with decimation

November 18, 2010 Coded in Mixed C and ASM for the Freescale DSP56F8xx
/* -------------- MovAvg.h begins -------------- */

#ifndef __MOVAVG_H__
#define __MOVAVG_H__

#include "types.h"

typedef struct
{
  unsigned int decimation;
  unsigned int deccnt;
  unsigned int BufHeadPtr;
  unsigned int BufLength;
  Frac16 multiplier;
  Frac16 ykm1;
}MovAvgparams;

/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes.   ;
;                                                                            ;
; Input:  Y0     = buffer length                                             ;
;         (R2+1) = decimation counter (used internally for data decimation)  ;
;         (R2+2) = index of buffer head                                      ;
;         (R2+3) = buffer length (filter order)                              ;
;         R3     = pointer to storage buffer for the past input samples      ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: R2                                                     ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data);

/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter.            ;
; The result yk is computed as the sum of the current and the past N input   ;
; samples multiplied by the multiplier value. Canonical moving average       ;
; filtering is obtained if multiplier = 1/(N+1).                             ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = xk (input sample)                                         ;
;         (R2)   = decimation (1 = no decimation, 2 = discard every other    ;
;                  sample, and so on)                                        ;
;         (R2+1) = decimation counter (used internally for data decimation)  ;
;         (R2+2) = index of buffer head                                      ;
;         (R2+3) = N (length of buffer and filter order)                     ;
;         (R2+4) = fractional multiplier                                     ;
;         (R2+5) = yk-1 (past output)                                        ;
;         R3     = Pointer to storage buffer for the N past input samples    ;
;                                                                            ;
; Output: Y0     = yk                                                        ;
;                                                                            ;
; Registers modified: A, X0, Y, R0, R3                                       ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data);

#endif //ifndef __MOVAVG_H__

/* -------------- MovAvg.h ends -------------- */

/* -------------- MovAvg.c begins -------------- */

/*---------------------------------------------------------------------------;
; Initialize buffer length, pointer and decimation divider of moving average ;
; filter; initialize memory buffer of moving average filter to all zeroes.   ;
;                                                                            ;
; Input:  Y0     = buffer length                                             ;
;         (R2+1) = decimation counter (used internally for data decimation)  ;
;         (R2+2) = index of buffer head                                      ;
;         (R2+3) = buffer length (filter order)                              ;
;         R3     = pointer to storage buffer for the past input samples      ;
;                                                                            ;
; Output: None                                                               ;
;                                                                            ;
; Registers modified: R2                                                     ;
;---------------------------------------------------------------------------*/
asm void MovAvgInit(unsigned int length, MovAvgparams *params, Frac16 *data){
                MOVE.W #0001,X:(R2+1)        //Initialize decimation divider
                CLR.W X:(R2+2)               //Initialize buffer head pointer
                MOVE.W Y0,X:(R2+3)           //Initialize buffer length
                REP Y0                       //Initialize past data to zero
                CLR.W X:(R3)+
                RTS                          //Return from subroutine
}

/*---------------------------------------------------------------------------;
; This function implements a moving average Nth order FIR filter.            ;
; The result yk is computed as the sum of the current and the past N input   ;
; samples multiplied by the multiplier value. Canonical moving average       ;
; filtering is obtained if multiplier = 1/(N+1).                             ;
; For optimal results it is advised to call this function with saturation    ;
; disabled.                                                                  ;
;                                                                            ;
; Input:  Y0     = xk (input sample)                                         ;
;         (R2)   = decimation (1 = no decimation, 2 = discard every other    ;
;                  sample, and so on)                                        ;
;         (R2+1) = decimation counter (used internally for data decimation)  ;
;         (R2+2) = index of buffer head                                      ;
;         (R2+3) = N (length of buffer and filter order)                     ;
;         (R2+4) = fractional multiplier                                     ;
;         (R2+5) = yk-1 (past output)                                        ;
;         R3     = Pointer to storage buffer for the N past input samples    ;
;                                                                            ;
; Output: Y0     = yk                                                        ;
;                                                                            ;
; Registers modified: A, X0, Y, R2, R3                                       ;
;---------------------------------------------------------------------------*/
asm Frac16 MovAvg(Frac16 xk, MovAvgparams *params, Frac16 *data){
                DEC.W X:(R2+1)               //Decrement decimation counter
                BEQ DoMovAvg                 //Decremented to zero?
                MOVE.W X:(R2+5),Y0           //No, output old value and exit
                BRA MovAvgDone
DoMovAvg:       MOVE.W X:(R2)+,X0            //Yes, reload decimation counter
                MOVE.W X0,X:(R2)+
                ADDA #1,SP                   //Preserve N (needed if called
                MOVE.W N,X:(SP)              //from an ISR)
                MOVE.W X:(R2)+,A             //Load index of buffer head into
                MOVE.W A1,N                  //address modifier register
                INC.W A X:(R2)+,Y1           //Point to next buffer location
                                             //and load buffer length
                CMP.W Y1,A                   //If buffer head index = buffer
                BNE MAIndexOK                //length, then reset buffer head
                CLR.W A1                     //index
MAIndexOK:      MOVE.W A1,X:(R2-2)           //Save buffer head index
                MOVE.W X:(R3+N),A1           //Load oldest buffer sample
                MOVE.W Y0,X:(R3+N)           //Save new input sample
                MOVE.W X:(R2)+,Y0            //Load multiplier
                MPY A1,Y0,A X:(R3)+,X0       //Accumulate oldest sample
                REP Y1                       //Accumulate the latest N samples
                MAC X0,Y0,A X:(R3)+,X0
                MOVE.W A,Y0                  //Return accumulation result
                MOVE.W Y0,X:(R2)             //Save new output sample
                MOVE.W X:(SP)-,X0            //Restore N
                MOVE.W X0,N
MovAvgDone:     RTS                          //Return from subroutine
}

/* -------------- MovAvg.c ends -------------- */

/* -------------- Usage example begins ------------- */

/* application specific constants */
#define MovAvgOrder 40                       //Order of moving average filter
#define MovAvgDecimation 100                 //Decimation factor for m.a.

/* application specific includes */
#include "MovAvg.h"

/* global variables */
MovAvgparams MAparams;
Frac16 MAdata[MovAvgOrder];
int FiltIn,MAOut;

/* initializations */
MAparams.decimation=MovAvgDecimation;
MAparams.multiplier=FRAC16((float)1/(MovAvgOrder+1));
MovAvgInit(MovAvgOrder,&MAparams,(Frac16*)&MAdata); //Initialize m.a. filter

/* Moving average filter function call */
  MAOut=MovAvg(FiltIn,&MAparams,(Frac16*)&MAdata); //Call moving av. filter

/* -------------- Usage example ends ------------- */

32 bit Proportional-Integral (PI) Regulator

November 18, 20101 comment Coded in Mixed C and ASM for the Freescale DSP56F8xx
/* -------------- 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 ------------- */