Second order IIR filter with decimation
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 ------------- */
