//IIR filter //using direct form //normalized to unity gain, and with a(1)=1 // a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb) // - a(2)*y(n-1) - ... - a(na+1)*y(n-na) #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 state variables -- must be in RAM! #pragma regalloc- int b1,b2,b3,a2,a3,xn, xn_1, xn_2, yn, yn_1, yn_2 ; #pragma regalloc+ //==Fast fixed multiply================================= #pragma warn- int multfix(int a,int b) begin #asm push r20 push r21 LDD R22,Y+2 ;load a LDD R23,Y+3 LD R20,Y ;load b LDD R21,Y+1 muls r23, r21 ; (signed)ah * (signed)bh mov r31, r0 ; mul r22, r20 ; al * bl mov r30, r1 ; ;mov r16, r0 mulsu r23, r20 ; (signed)ah * bl add r30, r0 ; adc r31, r1 ; mulsu r21, r22 ; (signed)bh * al add r30, r0 ; adc r31, r1 ; pop r21 pop r20 #endasm end //==Fixed Mult and Accumulate============================= // return = a*b + c ;; fixed format int macfix(int a, int b, int c) begin //r31:r30:r24 += r23:r22 * r21:r20 // #asm push r20 push r21 LDD R22,Y+4 ;load a LDD R23,Y+5 LDD R20,Y+2 ;load b LDD R21,Y+3 LD R30,Y ;load c to lsb result LDD R31,Y+1 ;and msb result clr r24 ;low order byte clr r27 ;permanent zero ;mac operation 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 ;end mac operation pop r21 pop r20 #endasm end //======================================================== //second order IIR -- "Direct Form II Transposed" // y(n) = b(1)*x(n) + b(2)*x(n-1) + b(3)*x(n-2) // - a(2)*y(n-1) - a(3)*y(n-2) //assumes a(1)=1 // a's and b's need to be global, in RAM and set to fixed point values // also input and output history values //example: // #pragma regalloc- // int b1,b2,b3,a2,a3, xn_1, xn_2, yn_1, yn_2 ; // #pragma regalloc+ // b1=0x0010; // a1=float2fix(-(value from matlab)) // // The following ASM code is equivalent to: // yy=0; yy = macfix(b1,xx,yy); // yy = macfix(b2,xn_1,yy); // yy = macfix(b3,xn_2,yy); // yy = macfix(-a2,yn_1,yy); // yy = macfix(-a3,yn_2,yy); // //update the state variables // xn_2 = xn_1; // xn_1 = xx; // yn_2 = yn_1; // yn_1 = yy; // return yy; int IIR2(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, _b1 ;load b1 from RAM lds R23, _b1+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _b2 ;load b2 from RAM lds R23, _b2+1 lds R20, _xn_1 ;load x(n-1) from RAM lds R21, _xn_1+1 mult_acc ; b2*x(n-1) lds R22, _b3 ;load b3 from RAM lds R23, _b3+1 lds R20, _xn_2 ;load x(n-2) from RAM lds R21, _xn_2+1 mult_acc ; b3*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 #pragma warn+ //======================================================== 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"); prod = 0x0a00; //2-pole butterworth lowpass, cutoff=0.25 (1/4 Nyquist freq), gain=1 b1 = float2fix(0.098) ; b2 = float2fix(0.195) ; b3 = 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 lowpass, cutoff=0.1 (1/10 Nyquist freq), gain=1 // b1 = float2fix(0.0201) ; // b2 = float2fix(0.0402) ; // b3 = float2fix(0.0201) ; // a2 = float2fix(1.561) ; //note that sign is negated from design input // a3 = float2fix(-.6414) ; //note that sign is negated from design input while(1) begin printf("\r\nenter a signed float:") ; scanf("%s",fInString); cIn = atof(fInString); printf("%f",cIn); cInFix = float2fix(cIn); printf(" fixed rep:%04x\r\n",cInFix); printf("enter a float:") ; scanf("%s",fInString); fIn = atof(fInString); printf("%f",fIn); fInFix = float2fix(fIn); printf(" fixed rep:%04x\r\n",fInFix); macprod = macfix(fInFix, cInFix, prod) ; printf("%f*%f+%f=%f", cIn, fIn, fix2float(prod), fix2float(macprod)) ; printf(" fixed rep:%04x\r\n", macprod) ; prod = macprod; //impulse response of IIR2 filter xn=0x1000; //test impulse amplitude = 16 for (i=0;i<16;i++) begin out = IIR2(xn); printf("%f\n\r", fix2float(out)) ; xn=0; end //============================================ TCCR1B = 1 ; TCNT1 = 0; junk = macfix(fInFix, cInFix, prod) ; TCCR1B = 0; printf("macfix cycles=%d\n\r",TCNT1) ; //============================================ TCCR1B = 1 ; TCNT1 = 0; junk = IIR2(xn) ; TCCR1B = 0; printf("IIR cycles=%d\n\r",TCNT1) ; //============================================ end end