//Butterworth 4 pole bandpass filter // y(n) = b(1)*x(n) + 2*b(1)*x(n-1) + b(1)*x(n-2) // - a(2)*y(n-1) - a(3)*y(n-2) // optimized #include #include #include //I like these definitions #define begin { #define end } #define float2fix(a) ((int)((a)*256.0)) #define fix2float(a) ((float)(a)/256.0) //IIR stuff #pragma regalloc- int b1,a2,a3,a4,a5; //filter constants int xn, xn_1, xn_2, xn_3, xn_4, ; //input state int yn, yn_1, yn_2, yn_3, yn_4 ; //output state #pragma regalloc+ //======================================================== int Butter4band(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm .macro mult_acc ;r31:r30:r24 += r23:r22 * r21:r20 muls r23, r21 ; (signed)ah * (signed)bh add r31, r0 mul r22, r20 ; al * bl add r24, r0 adc r30, r1 adc r31, r27 mulsu r23, r20 ; (signed)ah * bl add r30, r0 adc r31, r1 mulsu r21, r22 ; (signed)bh * al add r30, r0 adc r31, r1 .endm push r20 ;save parameter regs push r21 clr r27 ;permanent zero clr r24 ;clear 24 bit result reg; msb to lsb => r31:r30:r24 clr r30 clr r31 lds R22, _xn_2 ;load x(n-2) from RAM lds R23, _xn_2+1 lsl r22 ;mult by two rol r23 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sub r20, r22 ;form xx-2x(n-2) sbc r21, r23 lds R22, _xn_4 ;load x(n-4) from RAM lds R23, _xn_4+1 add r20, r22 ;form xx-2x(n-2)+x(n-4) adc r21, r23 lds R22, _b1 ;load b1 from RAM lds R23, _b1+1 mult_acc ; b1*(xx-2*x(n-2)+x(n-4)) lds R22, _a2 ;load -a2 from RAM lds R23, _a2+1 lds R20, _yn_1 ;load y(n-1) from RAM lds R21, _yn_1+1 mult_acc ; -a2*y(n-1) lds R22, _a3 ;load -a3 from RAM lds R23, _a3+1 lds R20, _yn_2 ;load y(n-2) from RAM lds R21, _yn_2+1 mult_acc ; -a3*y(n-2) lds R22, _a4 ;load -a4 from RAM lds R23, _a4+1 lds R20, _yn_3 ;load y(n-3) from RAM lds R21, _yn_3+1 mult_acc ; -a4*y(n-3) lds R22, _a5 ;load -a5 from RAM lds R23, _a5+1 lds R20, _yn_4 ;load y(n-3) from RAM lds R21, _yn_4+1 mult_acc ; -a5*y(n-4) ;update the state variables lds R20, _xn_3 ;load x(n-3) from RAM lds R21, _xn_3+1 sts _xn_4, r20 ;store x(n-4) to RAM sts _xn_4+1, R21 lds R20, _xn_2 ;load x(n-2) from RAM lds R21, _xn_2+1 sts _xn_3, r20 ;store x(n-3) to RAM sts _xn_3+1, R21 lds R20, _xn_1 ;load x(n-1) from RAM lds R21, _xn_1+1 sts _xn_2, r20 ;store x(n-2) to RAM sts _xn_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _xn_1, r20 ;store x(n-1) to RAM sts _xn_1+1, R21 lds R20, _yn_3 ;load y(n-3) from RAM lds R21, _yn_3+1 sts _yn_4, R20 ;store y(n-4) to RAM sts _yn_4+1, R21 lds R20, _yn_2 ;load y(n-2) from RAM lds R21, _yn_2+1 sts _yn_3, R20 ;store y(n-3) to RAM sts _yn_3+1, R21 lds R20, _yn_1 ;load y(n-1) from RAM lds R21, _yn_1+1 sts _yn_2, R20 ;store y(n-2) to RAM sts _yn_2+1, R21 sts _yn_1, r30 ;store new output as y(n-1) to RAM sts _yn_1+1, r31 pop r21 ;restore parameter regs pop r20 #endasm end /////////////////////////////////////////////////////// float fIn, cIn ; //an input float char fInString[16]; char i; signed int cInFix, fInFix, prod, macprod, junk, out; void main(void) begin //serial setop for debugging using printf, etc. UCSRB = 0x18 ; UBRRL = 103 ; putsf("\r\nStarting...\r\n"); //4-pole butterworth bandpass, cutoff=[0.25 0.35] b1 = float2fix(0.0201) ; a2 = float2fix(2.119) ; //note that sign is negated from design input a3 = float2fix(-2.695) ; //note that sign is negated from design input a4 = float2fix(1.692) ; //note that sign is negated from design input a5 = float2fix(-0.6414) ; //note that sign is negated from design input while(1) begin printf("\r\n enter 1 to run Butterworth:") ; scanf("%s",fInString); cIn = atof(fInString); printf("\r\n"); //impulse response of IIR2 filter xn=0x1000; //test impulse amplitude = 16 for (i=0;i<16;i++) begin out = Butter4band(xn); printf("%f\n\r", fix2float(out)) ; xn=0; end //============================================ TCCR1B = 1 ; TCNT1 = 0; junk = Butter4band(xn) ; TCCR1B = 0; printf("cycles=%d\n\r",TCNT1) ; //============================================ end end