Moving average filter with decimation
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 ------------- */
