📄 iir_filt.c
字号:
/*****************************************************************************
* Second-Order IIR Filter Module
*
* Written for "Digital SIgnal Processing with the PIC16C74" Application Note.
*
* This routine implements an IIR filter using a second order difference
* eqauation of the form:
*
* y(n) = b0*x(n)+b1*x(n-1)+b2*x(n-2)+a1*y(n-1)+a2*y(n-2)
*
* D. Mostowfi 3/95
*****************************************************************************/
#include "dbl_math.c"
bits x_n; /* input sample x(n) */
unsigned long y_n; /* output sample y(n) */
unsigned long x_n_1; /* x(n-1) */
unsigned long x_n_2; /* x(n-2) */
unsigned long y_n_1; /* y(n-1) */
unsigned long y_n_2; /* y(n-2) */
char rmndr_h; /* high byte of remainder from multiplies */
char rmndr_l; /* low byte of remainder from multiplies */
#define A1_H 0xd2 /* filter coefficients */
#define A1_L 0x08 /* for 60Hz notch filter */
#define A2_H 0x11 /* Fs= 1kHz */
#define A2_L 0x71
#define B0_H 0x18
#define B0_L 0xbb
#define B1_H 0xd2
#define B1_L 0x08
#define B2_H 0x18
#define B2_L 0xb9
/******************************************************************************
* Filter initialization - clears all taps in memory.
*
* usage:
* - call init_filter()
* use at program initialization
******************************************************************************/
void init_filter(){
#asm
clrf y_n ; clear output value
clrf y_n+1 ;
clrf y_n_1 ; and all filter "taps"
clrf y_n_1+1 ;
clrf y_n_2 ;
clrf y_n_2+1 ;
clrf x_n_1 ;
clrf x_n_1+1 ;
clrf x_n_2 ;
clrf x_n_2+1 ;
#endasm
}
/******************************************************************************
* Assembly language subroutines for main filter() function
******************************************************************************/
#asm
;
; Add Remainder subroutine - adds remainder from multiplies to ACCc
;
add_rmndr:
btfss sign,7 ; check if number is negative
goto add_r_start ; go to add_r_start if not
comf ACCcLO ; if so, negate number in ACC
incf ACCcLO ;
btfsc STATUS,Z ;
decf ACCcHI ;
comf ACCcHI ;
btfsc STATUS,Z ;
comf ACCbLO ;
incf ACCbLO ;
btfsc STATUS,Z ;
decf ACCbHI ;
comf ACCbHI ;
add_r_start:
movf rmndr_l,W ; get low byte of remainder
addwf ACCcLO ; and add to ACCcLO
btfsc STATUS,C ; check for overflow
incf ACCcHI ; if overflow, increment ACCcHI
movf rmndr_h,W ; get high byte of remainder
addwf ACCcHI ; and add to ACCcHI
btfsc STATUS,C ; check for overflow
incf ACCbLO ; if overflow, increment ACCbLO
btfss sign,7 ; check if result negative
goto add_r_done ; if not, go to add_r_done
comf ACCcLO ; if so, negate result
incf ACCcLO ;
btfsc STATUS,Z ;
decf ACCcHI ;
comf ACCcHI ;
btfsc STATUS,Z ;
comf ACCbLO ;
incf ACCbLO ;
btfsc STATUS,Z ;
decf ACCbHI ;
comf ACCbHI ;
add_r_done: ;
retlw 0 ; done
;
; Decimal Adjust Subroutine - used after each Q15 multiply to convert Q30 result
; to Q15 number
dec_adjust:
bcf sign,7 ; clear sign
btfss ACCbHI,7 ; test if number is negative
goto adjust ; go to adjust if not
bsf sign,7 ; set sign if negative
comf ACCcLO ; and negate number
incf ACCcLO
btfsc STATUS,Z
decf ACCcHI
comf ACCcHI
btfsc STATUS,Z
comf ACCbLO
incf ACCbLO
btfsc STATUS,Z
decf ACCbHI
comf ACCbHI
adjust:
rlf ACCcHI ; rotate ACC left 1 bit
rlf ACCbLO ;
rlf ACCbHI ;
btfss sign,7 ; check if result should be negative
goto adj_done ; if not, done
comf ACCbLO ; if result negative, negate ACC
incf ACCbLO
btfsc STATUS,Z
decf ACCbHI
comf ACCbHI
adj_done:
retlw 0 ; done
;
; Output Scaling Routine - used to scale output samples by factors of
; 2, 4, or 8 at end of filter routine
;
scale_y_n:
bcf sign,7 ; clear sign,7
btfss y_n+1,7 ; test if y(n) negative
goto start_scale ; go to start_scale if not
bsf sign,7 ; set sign,7 if negative
comf y_n ; and compliment y(n)
incf y_n ;
btfsc STATUS,Z ;
decf y_n+1 ;
comf y_n+1 ;
start_scale:
bcf STATUS,C ; clear carry
rlf y_n+1 ; and rotate y(n) left
rlf y_n ;
bcf STATUS,C ;
rlf y_n+1 ;
rlf y_n ;
bcf STATUS,C ;
rlf y_n+1 ;
rlf y_n ;
btfss sign,7 ; test if result is negative
goto scale_y_done ; go to scale_y_done if not
comf y_n ; negate y(n) if result is negative
incf y_n ;
btfsc STATUS,Z ;
decf y_n+1 ;
comf y_n+1 ;
scale_y_done:
retlw 0 ; done
#endasm
/******************************************************************************
* Filter function - filter takes current input sample, x(n), and outputs next
* output sample, y(n).
*
* usage:
* - write sample to be filtered to x_n
* - call filter()
* - output is in MSB of y_n (y_n=MSB, y_n+1=LSB)
*
******************************************************************************/
void filter(){
#asm
clrf y_n ; clear y(n) before starting
clrf y_n+1 ;
clrf ACCbLO ; move x(n) to ACCbHI
movf x_n,W ; (scale 8 bit - 16 bit input)
movwf ACCbHI ;
movlw B0_H ; get coefficient b0
movwf ACCaHI ; y(n)=b0*x(n)
movlw B0_L ;
movwf ACCaLO ;
call D_mpyF ;
movf ACCcHI,W ; save remainder from multiply
movwf rmndr_h ;
movf ACCcLO,W ;
movwf rmndr_l ;
call dec_adjust ;
movf ACCbHI,W ;
movwf y_n+1 ;
movf ACCbLO,W ;
movwf y_n ;
movlw B1_H ; get coefficient b1
movwf ACCaHI ; y(n)=y(n)+b1*x(n-1)
movlw B1_L ;
movwf ACCaLO ;
movf x_n_1+1,W ;
movwf ACCbHI ;
movf x_n_1,W ;
movwf ACCbLO ;
call D_mpyF ;
call add_rmndr ; add in remainder from previous multiply
movf ACCcHI,W ; and save new remainder
movwf rmndr_h ;
movf ACCcLO,W ;
movwf rmndr_l ;
call dec_adjust ;
movf y_n+1,W ;
movwf ACCaHI ;
movf y_n,W ;
movwf ACCaLO ;
call D_add ;
movf ACCbHI,W ;
movwf y_n+1 ;
movf ACCbLO,W ;
movwf y_n ;
movlw B2_H ; get coefficient b2
movwf ACCaHI ; y(n)=y(n)+b2*x(n-2)
movlw B2_L ;
movwf ACCaLO ;
movf x_n_2+1,W ;
movwf ACCbHI ;
movf x_n_2,W ;
movwf ACCbLO ;
call D_mpyF ;
call add_rmndr ; add in remainder from previous multiply
movf ACCcHI,W ; and save new remainder
movwf rmndr_h ;
movf ACCcLO,W ;
movwf rmndr_l ;
call dec_adjust ;
movf y_n+1,W ;
movwf ACCaHI ;
movf y_n,W ;
movwf ACCaLO ;
call D_add ;
movf ACCbHI,W ;
movwf y_n+1 ;
movf ACCbLO,W ;
movwf y_n ;
movlw A1_H ; get coefficient a1
movwf ACCaHI ; y(n)=y(n)+a1*y(n-1)
movlw A1_L ;
movwf ACCaLO ;
movf y_n_1+1,W ;
movwf ACCbHI ;
movf y_n_1,W ;
movwf ACCbLO ;
call D_mpyF ;
call add_rmndr ; add in remainder from previous multiply
movf ACCcHI,W ; and save new remainder
movwf rmndr_h ;
movf ACCcLO,W ;
movwf rmndr_l ;
call dec_adjust ;
movf y_n+1,W ;
movwf ACCaHI ;
movf y_n,W ;
movwf ACCaLO ;
call D_sub ;
movf ACCbHI,W ;
movwf y_n+1 ;
movf ACCbLO,W ;
movwf y_n ;
movlw A2_H ; get coefficient a2
movwf ACCaHI ; y(n)=y(n)+a2*y(n-2)
movlw A2_L ;
movwf ACCaLO ;
movf y_n_2+1,W ;
movwf ACCbHI ;
movf y_n_2,W ;
movwf ACCbLO ;
call D_mpyF ;
call add_rmndr ;
call dec_adjust ;
movf y_n+1,W ;
movwf ACCaHI ;
movf y_n,W ;
movwf ACCaLO ;
call D_sub ;
movf ACCbHI,W ;
movwf y_n+1 ;
movf ACCbLO,W ;
movwf y_n ;
movf x_n_1,W ; x(n-2)=x(n-1)
movwf x_n_2 ;
movf x_n_1+1,W ;
movwf x_n_2+1 ;
movf x_n,W ; x(n-1)=x(n)
movwf x_n_1+1 ;
clrf x_n_1 ;
movf y_n_1,W ; y(n-2)=y(n-1)
movwf y_n_2 ;
movf y_n_1+1,W ;
movwf y_n_2+1 ;
movf y_n,W ; y(n-1)=y(n)
movwf y_n_1 ;
movf y_n+1,W ;
movwf y_n_1+1 ;
call scale_y_n ;
movf y_n+1,W ; shift lsb of y_n to msb
movwf y_n ;
#endasm
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -