Code

Second order IIR filter with decimation

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

This library implements a C-callable ASM second order IIR filter for the Freescale DSP56F800E family of digital signal controllers.

The filter has a configurable decimation factor, useful for filtering slowly varying signals with high sampling rates.
 
Source includes the header file for inclusion in C applications as well as the code itself and an usage example in C.

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