Code

Moving average filter with decimation

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

This library implements a C-callable ASM moving average filter for the Freescale DSP56F800E family of digital signal controllers.

The filter has a configurable decimation factor, useful for correctly averaging 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.

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