//Butterworth lowpass 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 so that you only need to specify // b1, a2, a3 #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,xn, xn_1, xn_2, yn, yn_1, yn_2 ; #pragma regalloc+ //======================================================== int Butter2low(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_1 ;load x(n-1) from RAM lds R23, _xn_1+1 lsl r22 ;mult by two rol r23 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 add r20, r22 ;form xx+2x(n-1) adc r21, r23 lds R22, _xn_2 ;load x(n-2) from RAM lds R23, _xn_2+1 add r20, r22 ;form xx+2x(n-1)+x(n-2) adc r21, r23 lds R22, _b1 ;load b1 from RAM lds R23, _b1+1 mult_acc ; b1*(xx+2*x(n-1)+x(n-2)) 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 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_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 //======================================================== int Butter2high(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_1 ;load x(n-1) from RAM lds R23, _xn_1+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-1) sbc r21, r23 lds R22, _xn_2 ;load x(n-2) from RAM lds R23, _xn_2+1 add r20, r22 ;form xx-2x(n-1)+x(n-2) adc r21, r23 lds R22, _b1 ;load b1 from RAM lds R23, _b1+1 mult_acc ; b1*(xx-2*x(n-1)+x(n-2)) 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 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_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 //======================================================== int Butter2band(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 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sub r20, r22 ;form xx-x(n-2) sbc r21, r23 lds R22, _b1 ;load b1 from RAM lds R23, _b1+1 mult_acc ; b1*(xx-x(n-2)) 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 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_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"); //2-pole butterworth lowpass, cutoff=0.25 (1/4 Nyquist freq), gain=1 b1 = float2fix(0.098) ; a2 = float2fix(0.943) ; //note that sign is negated from design input a3 = float2fix(-0.333) ; //note that sign is negated from design input //2-pole butterworth highpass, cutoff=0.25 b1 = float2fix(0.569) ; a2 = float2fix(0.943) ; //note that sign is negated from design input a3 = float2fix(-.333) ; //note that sign is negated from design input //2-pole butterworth bandpass, cutoff=[0.25 0.35] b1 = float2fix(0.137) ; a2 = float2fix(1.028) ; //note that sign is negated from design input a3 = float2fix(-0.727) ; //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 = Butter2band(xn); printf("%f\n\r", fix2float(out)) ; xn=0; end //============================================ TCCR1B = 1 ; TCNT1 = 0; junk = Butter2band(xn) ; TCCR1B = 0; printf("cycles=%d\n\r",TCNT1) ; //============================================ end end