// Speech Recognition reference code... // This is a srecog code to compare fingerprints and create PWM signals. // Authors: Andre Harison (avh8@cornell.edu) // Chirag Shah (css34@cornell.edu) // Semester:spring 2006 // This is similar funtion as template #include #include #include #include #include "dict.h" // dictonary file created using template code #define begin { #define end } #define NSAMPLES_PER_INTERVAL 250 #define NSAMPLES 4000 #define NDIM 160 #define NWORDS 5 #define NCOP 3 // Functionality of a car #define go 0 #define left 1 #define right 2 #define stop 3 #define back 4 #pragma regalloc- // Register Allocation int x11_1, x11_2, x12_1, x12_2; // Filter1 Inputs int y11_1, y11_2, y12_1, y12_2; // Filter1 Outputs int x21_1, x21_2, x22_1, x22_2; // Filter2 Inputs int y21_1, y21_2, y22_1, y22_2; // Filter2 Outputs int x31_1, x31_2, x32_1, x32_2; // Filter3 Inputs int y31_1, y31_2, y32_1, y32_2; // Filter3 Outputs int x41_1, x41_2, x42_1, x42_2; // Filter4 Inputs int y41_1, y41_2, y42_1, y42_2; // Filter4 Outputs int x51_1, x51_2, x52_1, x52_2; // Filter5 Inputs int y51_1, y51_2, y52_1, y52_2; // Filter5 Outputs int x61_1, x61_2, x62_1, x62_2; // Filter6 Inputs int y61_1, y61_2, y62_1, y62_2; // Filter6 Outputs int x71_1, x71_2, x72_1, x72_2; // Filter7 Inputs int y71_1, y71_2, y72_1, y72_2; // Filter7 Outputs int x81_1, x81_2, x82_1, x82_2; // Filter8 Inputs int y81_1, y81_2, y82_1, y82_2; // Filter8 Outputs int x91_1, x91_2, x92_1, x92_2; // Filter9 Inputs int y91_1, y91_2, y92_1, y92_2; // Filter9 Outputs int x101_1, x101_2, x102_1, x102_2; // Filter10 Inputs int y101_1, y101_2, y102_1, y102_2; // Filter10 Outputs // Stores Each filters output. unsigned int y1=0, y2=0, y3=0, y4=0, y5=0, y6=0,y7=0,y8=0,y9=0,y10=0; // vairable to check filterout before squaring. unsigned int gainchk; #pragma regalloc+ // Register allcation finish. int word[16][10]; char nms_d; // keeps track of if noise measurement is done char it; // iteration counter unsigned int noise1, noise2, noise3; // sample noise measurement values char searchindx; // in main|initialization statement set to zero char WRDFND; // word has been found char Ain; // value of ADCH (read in ADC interrupt unsigned int threshold ; // threshold for word vs. noise (3*avg noise ampltiude char i, j,k, l, m, ; char new_smpl; // new value in from ADC int fingerprint[NDIM]; int y; char z; // misc temp var int input; double rtop, rxbot, rybot, r, dicmean, fingmean, sqval; // variables for correlation calculation double xdiff, ydiff; double res, mindix; unsigned long int tmp1; long int tmp; // storing difference values from euclidean distance unsigned long int result; unsigned long int mindis; char minword; void initialize(); void meas_noise(); // Measure noise analyses void get_sample(); void lookup(); int multfix(int a,int b); //Fixed point multiplication int macfix(int a, int b, int c); // Fixed point multiplier accumulator long int ric = 2333264; unsigned int cycle; char segcnt, samcnt; // 250 samcnt & 32 segcnt // #define int2fix(a) (((int)(a))<<8) //Convert char to fix. a is a char #define fix2int(a) ((signed char)((a)>>8)) //Convert fix to char. a is an int #define fix2uint(a) ((unsigned char)((a)>>8)) //Convert fix to char. a is an int #define float2fix(a) ((int)((a)*256.0)) #define fix2float(a) ((float)(a)/256.0) #pragma regalloc- //We are not using filter1 since it has higher noise level. int A112 = 435; int A113 = -186; int B111 = 10; int B112 = -14; int B113 = 10; // int A122 = 480; int A123 = -230; int B121 = 2459; int B122 = -4637; int B123 = 2459; int G1 = 140; //7 // Filter2: 1st 2nd order coefficient int A212 = 451; int A213 = -248; int B211 = 21; int B212 = -32; int B213 = 21; // Filter2: 2nd 2nd order coefficient int A222 = 458; int A223 = -248; int B221 = 2225; int B222 = -4285; int B223 = 2225; int G2 = 80; //4 // Filter2 gain // Filter3: 1st 2nd order coefficient int A312 = 355; int A313 = -248; int B311 = 27; int B312 = -29; int B313 = 27; //Filter3: 2nd 2nd order coefficient int A322 = 366; int A323 = -248; int B321 = 1090; int B322 = -1826; int B323 = 1090; int G3 = 120; //6 //Filter3 gain //Filter4: 1st 2nd order coefficient int A412 = 224; int A413 = -248; int B411 = 31; int B412 = -15; int B413 = 31; //Filter4: 2nd 2nd order coefficient int A422 = 239; int A423 = -248; int B421 = 762; int B422 = -965; int B423 = 762; int G4 = 140; //7 //Filter4 gain //Filter5: 1st 2nd order coefficient int A512 = 72; int A513 = -248; int B511 = 34; int B512 = 4; int B513 = 34; //Filter5: 2nd 2nd order coefficient int A522 = 88; int A523 = -248; int B521 = 633; int B522 = -464; int B523 = 633; int G5 = 160; //8 //Filter5 gain //Filter6: 1st 2nd order coefficient int A612 = -72; int A613 = -248; int B611 = 34; int B612 = -4; int B613 = 34; //Filter6: 2nd 2nd order coefficient int A622 = -88; int A623 = -248; int B621 = 633; int B622 = 464; int B623 = 633; int G6 = 160; //8 //Filter6 gain //Filter7: 1st 2nd order coefficient int A712 = -224; int A713 = -248; int B711 = 31; int B712 = 15; int B713 = 31; //Filter7: 2nd 2nd order coefficient int A722 = -239; int A723 = -248; int B721 = 762; int B722 = 965; int B723 = 762; int G7 = 140; //7 //Filter7 gain //Filter8: 1st 2nd order coefficient int A812 = -355; int A813 = -248; int B811 = 27; int B812 = 29; int B813 = 27; //Filter8: 2nd 2nd order coefficient int A822 = -366; int A823 = -248; int B821 = 1090; int B822 = 1826; int B823 = 1090; int G8 = 120; //6 //Filter8 gain //Filter9: 1st 2nd order coefficient int A912 = -451; int A913 = -248; int B911 = 21; int B912 = 32; int B913 = 21; //Filter9: 2nd 2nd order coefficient int A922 = -458; int A923 = -248; int B921 = 2225; int B922 = 4285; int B923 = 2225; int G9 = 80; //4 //Filter10: 1st 2nd order coefficient int A1012 = -435; int A1013 = -186; int B1011 = 10; int B1012 = 14; int B1013 = 10; //Filter10: 2nd 2nd order coefficient int A1022 = -480; int A1023 = -230; int B1021 = 2459; int B1022 = 4637; int B1023 = 2459; int G10 = 140; //7 #pragma regalloc+ // // //int filter1(int x); // 0 - 200Hz Low pass Filter int filter2(int x); // 200Hz - 400Hz int filter3(int x); // 400Hz - 600Hz int filter4(int x); // 600Hz - 800Hz int filter5(int x); // 800Hz - 1000Hz int filter6(int x); // 1000Hz - 1200Hz int filter7(int x); // 1200Hz - 1400Hz int filter8(int x); // 1400Hz - 1600Hz int filter9(int x); // 1600Hz - 1800Hz int macfix(int a, int b, int c); // multiplier and accum // //////////////////////////////////////////////////////// // Timer0 interrupt used to measure noise and to take ADC data interrupt [TIM0_COMP] void noiseanaly(void) begin // To start ADC again Ain = ADCH; new_smpl = 1; ADCSR.6=1; if (z == 2) { TCCR0 = 0b00001011;// Prescale 64,clear on compare match WGM01 = 1,WGM00 = 0 TCNT0 = 0; OCR0 = 62;// This is sampling about at 4300Hz z=0; } end ////////////////////////////////////////////////////////////////////////////// void initialize() { searchindx = 0; WRDFND = 0; // word found DDRD = 0XFF; TCCR1A = 0b11110010; TCCR1B = 0b00010010; // prescale /8 and CTC OCR1A = 18500; OCR1B = 18500; //This is wrong ICR1H = 0X4E; ICR1L = 0X20; //TCCR2 = 0b01110111; // prescale:1024 //OCR2 = 0 ; TIMSK = 0b00000010; // Setup the interrupt for counter0 . Ain = 0; //Adc data variable new_smpl = 0; // flag to indicate sample from adc nms_d = 0; it =0; // setup timer interrupt to trigger every 125us segcnt = 0; samcnt = 0; ADMUX = 0b00100000; // Initialize ADC mux value ADCSR = 0b11000111; // Watch out Bit 4. DDRB = 0xFF; // Blink LED PORTB = 0xFF; DDRC = 0x00; // Connected to switches //setup for Tranm UCSRB = 0x18 ; // UART to setup TX and Rx UBRRL = 103 ; // Baud Rate for mega32. TCCR0 = 0b00001011; // prescale divide by 64, 122Hz pwm signal. OCR0 = 62; //TCCR2 = 0b01110100; // phase correct pwm, prescale divide by 256, 122Hz pwm signal. printf("\r\nStarting...\r\n"); #asm sei #endasm meas_noise(); // To measureNoise //PORTB = 0xFE; // Indicates Noise measurement done! } ////////////////////////////////////////////////////////////////////////////// // to measure the noise in the room at prgram start up. void meas_noise(void) { i = 0; while (nms_d == 0) { // loop until noise measurement is done if (it == 0){ if (new_smpl == 1) { new_smpl =0; noise1 += Ain; i++; } } else if (it == 1) { if (new_smpl == 1) { new_smpl = 0; noise2 += Ain; i++; } } if (i == 255) { i = 0; it++; // iterate if (it > 2) nms_d =1; TCNT0 = 0; TCCR0 = 0b00001101; // in timer1 interrupt when match occurs turn off timer1 and start timer0 again. OCR0 = 255; z = 2; } else if (it == 2) { if (new_smpl == 1) { new_smpl = 0; noise3 += Ain; i++; } } } if (noise1 > noise2) { if (noise2 > noise3) { threshold = noise2 <<2; } else if (noise1 > noise3 ) { threshold = noise3 <<2; } else threshold = noise1 <<2; } else if ( noise1 > noise3) { threshold = noise1 << 2; } else if (noise2 > noise3) { threshold = noise3 <<2; } else threshold = noise2 <<2; } void get_sample(void) { if (WRDFND == 0) { // word is still not found if (((unsigned) int2fix(Ain) > threshold) ) { //&& (PINC == 0x7F) WRDFND = 1; i = 0; // PORTB = 0x7F; } } } //////////////////////////////////////////////////////////////////////////////// /* // ********This is Correlation Funtion which does correlation between fingerprint and // dictionary words but we did not use it at the end. ********** // compares the dictinary word with fingerprint words void corr_lookup() { // compute the linear correlation of the fingerprints res= 0; mindix = 0; // The first for loop assings mindis to be initial distance from first word. for (j = 0; j < 10; j++) { // calculate mean for filter j dicmean = 0; fingmean = 0; rtop = 0; rxbot = 0;` rybot = 0; for ( i = (j<<4); i < 16+(j<<4); i++) { dicmean = dicmean + (double) dic[0][0][i]; fingmean = fingmean + (double) fingerprint[i]; } dicmean = dicmean/16; fingmean = fingmean/16; for (i = (j<<4); i< 16+(j<<4); i++) { // variables rtop, rxbot, rybot, r, dicmean, fingmean, sqval xdiff = (dic[0][0][i] - dicmean); ydiff = (fingerprint[j] - fingmean); sqval = xdiff*ydiff; rtop = rtop + sqval; sqval = xdiff*xdiff; rxbot = rxbot + sqval; sqval = ydiff*ydiff; rybot = rybot + sqval; } rybot = sqrt(rybot); rxbot = sqrt(rxbot); r = rtop/rxbot; r = r/rybot; r = abs(r); printf("%f\r\n",rtop); mindix = mindix + r; } for (m = 0; m mindix ) { mindix = res; minword = k; } printf("\r\nResult "); printf("%f\r\n",res); } } printf("mindis %f\r\n",mindix); } */ //************ Correlation Function ends here!*********** void lookup() { // by default assume the first word... result = 0; mindis = 0; minword = 0; // The first for loop assings mindis to be initial distance from first word. for(j = 16; j < NDIM; j++){ tmp = dic[0][j] - fingerprint[j]; mindis = mindis + (long int)tmp1; } for(j = 0; j < NWORDS; j++){ result = 0; for( i=16; i < NDIM; i++ ) { tmp = dic[j][i] - fingerprint[i]; tmp1 = abs(tmp); result = result + (long int) tmp1; // ~~~~~~~~~~~ Watch Out! Yellow Alert\ } // To check the result is the minimum distance. if( result < mindis ) { mindis = result; minword = j; } printf("\r\nResult "); printf("%ld\r\n",result); } printf("mindis %ld\r\n",mindis); } /*This is car control function which sends different PWM signal to Cars depending on values of minword set from lookup() function. */ void carcontrol(void){ if(minword == go){ OCR1A = 18455; } else if(minword == left){ OCR1B = 18000; //OCR2 = 250; // 10% of duty cycle. } else if(minword == right){ OCR1B = 19000; //OCR2 = 235; } else if(minword == stop){ OCR1A = 18500; OCR1B = 18500; //OCR2= 243; } else if(minword == back){ OCR1A = 18550; // 6.5% duty cycle } else{ OCR1A = 18500; // Default case always stops OCR1B = 18500; //OCR2=243; } } ///////////////////////////////////////////////////////////////////// //======================================================== //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 filter1(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, _B111 ;load b1 from RAM lds R23, _B111+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B112 ;load b2 from RAM lds R23, _B112+1 lds R20, _x11_1 ;load x(n-1) from RAM lds R21, _x11_1+1 mult_acc ; b2*x(n-1) lds R22, _B113 ;load b3 from RAM lds R23, _B113+1 lds R20, _x11_2 ;load x(n-2) from RAM lds R21, _x11_2+1 mult_acc ; b3*x(n-2) lds R22, _A112 ;load -a2 from RAM lds R23, _A112+1 lds R20, _y11_1 ;load y(n-1) from RAM lds R21, _y11_1+1 mult_acc ; -a2*y(n-1) lds R22, _A113 ;load -a3 from RAM lds R23, _A113+1 lds R20, _y11_2 ;load y(n-2) from RAM lds R21, _y11_2+1 mult_acc ; -a3*y(n-2) lds R20, _x11_1 ;load x(n-1) from RAM lds R21, _x11_1+1 sts _x11_2, r20 ;store x(n-2) to RAM sts _x11_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x11_1, r20 ;store x(n-1) to RAM sts _x11_1+1, R21 lds R20, _y11_1 ;load y(n-1) from RAM lds R21, _y11_1+1 sts _y11_2, R20 ;store y(n-2) to RAM sts _y11_2+1, R21 sts _y11_1, r30 ;store new output as y(n-1) to RAM sts _y11_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B121 ;load b1 from RAM lds R23, _B121+1 lds R20, _y11_1 ;load input parameter xx from stack lds R21, _y11_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B122 ;load b2 from RAM lds R23, _B122+1 lds R20, _x12_1 ;load x(n-1) from RAM lds R21, _x12_1+1 mult_acc ; b2*x(n-1) lds R22, _B123 ;load b3 from RAM lds R23, _B123+1 lds R20, _x12_2 ;load x(n-2) from RAM lds R21, _x12_2+1 mult_acc ; b3*x(n-2) lds R22, _A122 ;load -a2 from RAM lds R23, _A122+1 lds R20, _y12_1 ;load y(n-1) from RAM lds R21, _y12_1+1 mult_acc ; -a2*y(n-1) lds R22, _A123 ;load -a3 from RAM lds R23, _A123+1 lds R20, _y12_2 ;load y(n-2) from RAM lds R21, _y12_2+1 mult_acc ; -a3*y(n-2) lds R20, _x12_1 ;load x(n-1) from RAM lds R21, _x12_1+1 sts _x12_2, r20 ;store x(n-2) to RAM sts _x12_2+1, R21 lds R20, _y11_1 ;load input parameter xx from stack lds R21, _y11_1+1 sts _x12_1, r20 ;store x(n-1) to RAM sts _x12_1+1, R21 lds R20, _y12_1 ;load y(n-1) from RAM lds R21, _y12_1+1 sts _y12_2, R20 ;store y(n-2) to RAM sts _y12_2+1, R21 sts _y12_1, r30 ;store new output as y(n-1) to RAM sts _y12_1+1, r31 ;*************************End of Second 2nd order IIR filter.************** ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G1 lds r21,_G1+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y1 lds r31,_y1+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 pop r21 ;restore parameter regs pop r20 #endasm end //*********************FILTER 2******************* int filter2(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, _B211 ;load b1 from RAM lds R23, _B211+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B212 ;load b2 from RAM lds R23, _B212+1 lds R20, _x21_1 ;load x(n-1) from RAM lds R21, _x21_1+1 mult_acc ; b2*x(n-1) lds R22, _B213 ;load b3 from RAM lds R23, _B213+1 lds R20, _x21_2 ;load x(n-2) from RAM lds R21, _x21_2+1 mult_acc ; b3*x(n-2) lds R22, _A212 ;load -a2 from RAM lds R23, _A212+1 lds R20, _y21_1 ;load y(n-1) from RAM lds R21, _y21_1+1 mult_acc ; -a2*y(n-1) lds R22, _A213 ;load -a3 from RAM lds R23, _A213+1 lds R20, _y21_2 ;load y(n-2) from RAM lds R21, _y21_2+1 mult_acc ; -a3*y(n-2) lds R20, _x21_1 ;load x(n-1) from RAM lds R21, _x21_1+1 sts _x21_2, r20 ;store x(n-2) to RAM sts _x21_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x21_1, r20 ;store x(n-1) to RAM sts _x21_1+1, R21 lds R20, _y21_1 ;load y(n-1) from RAM lds R21, _y21_1+1 sts _y21_2, R20 ;store y(n-2) to RAM sts _y21_2+1, R21 sts _y21_1, r30 ;store new output as y(n-1) to RAM sts _y21_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B221 ;load b1 from RAM lds R23, _B221+1 lds R20, _y21_1 ;load input parameter xx from stack lds R21, _y21_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B222 ;load b2 from RAM lds R23, _B222+1 lds R20, _x22_1 ;load x(n-1) from RAM lds R21, _x22_1+1 mult_acc ; b2*x(n-1) lds R22, _B223 ;load b3 from RAM lds R23, _B223+1 lds R20, _x22_2 ;load x(n-2) from RAM lds R21, _x22_2+1 mult_acc ; b3*x(n-2) lds R22, _A222 ;load -a2 from RAM lds R23, _A222+1 lds R20, _y22_1 ;load y(n-1) from RAM lds R21, _y22_1+1 mult_acc ; -a2*y(n-1) lds R22, _A223 ;load -a3 from RAM lds R23, _A223+1 lds R20, _y22_2 ;load y(n-2) from RAM lds R21, _y22_2+1 mult_acc ; -a3*y(n-2) lds R20, _x22_1 ;load x(n-1) from RAM lds R21, _x22_1+1 sts _x22_2, r20 ;store x(n-2) to RAM sts _x22_2+1, R21 lds R20, _y21_1 ;load input parameter xx from stack lds R21, _y21_1+1 sts _x22_1, r20 ;store x(n-1) to RAM sts _x22_1+1, R21 lds R20, _y22_1 ;load y(n-1) from RAM lds R21, _y22_1+1 sts _y22_2, R20 ;store y(n-2) to RAM sts _y22_2+1, R21 sts _y22_1, r30 ;store new output as y(n-1) to RAM sts _y22_1+1, r31 ;*************************End of Second 2nd order IIR filter.************** ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G2 lds r21,_G2+1 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 sts _gainchk,r30 sts _gainchk+1,r31 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y2 lds r31,_y2+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 pop r21 ;restore parameter regs pop r20 #endasm end //**************************FILTER 3************************** int filter3(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B311 ;load b1 from RAM lds R23, _B311+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B312 ;load b2 from RAM lds R23, _B312+1 lds R20, _x31_1 ;load x(n-1) from RAM lds R21, _x31_1+1 mult_acc ; b2*x(n-1) lds R22, _B313 ;load b3 from RAM lds R23, _B313+1 lds R20, _x31_2 ;load x(n-2) from RAM lds R21, _x31_2+1 mult_acc ; b3*x(n-2) lds R22, _A312 ;load -a2 from RAM lds R23, _A312+1 lds R20, _y31_1 ;load y(n-1) from RAM lds R21, _y31_1+1 mult_acc ; -a2*y(n-1) lds R22, _A313 ;load -a3 from RAM lds R23, _A313+1 lds R20, _y31_2 ;load y(n-2) from RAM lds R21, _y31_2+1 mult_acc ; -a3*y(n-2) lds R20, _x31_1 ;load x(n-1) from RAM lds R21, _x31_1+1 sts _x31_2, r20 ;store x(n-2) to RAM sts _x31_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x31_1, r20 ;store x(n-1) to RAM sts _x31_1+1, R21 lds R20, _y31_1 ;load y(n-1) from RAM lds R21, _y31_1+1 sts _y31_2, R20 ;store y(n-2) to RAM sts _y31_2+1, R21 sts _y31_1, r30 ;store new output as y(n-1) to RAM sts _y31_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B321 ;load b1 from RAM lds R23, _B321+1 lds R20, _y31_1 ;load input parameter xx from stack lds R21, _y31_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B322 ;load b2 from RAM lds R23, _B322+1 lds R20, _x32_1 ;load x(n-1) from RAM lds R21, _x32_1+1 mult_acc ; b2*x(n-1) lds R22, _B323 ;load b3 from RAM lds R23, _B323+1 lds R20, _x32_2 ;load x(n-2) from RAM lds R21, _x32_2+1 mult_acc ; b3*x(n-2) lds R22, _A122 ;load -a2 from RAM lds R23, _A122+1 lds R20, _y32_1 ;load y(n-1) from RAM lds R21, _y32_1+1 mult_acc ; -a2*y(n-1) lds R22, _A323 ;load -a3 from RAM lds R23, _A323+1 lds R20, _y32_2 ;load y(n-2) from RAM lds R21, _y32_2+1 mult_acc ; -a3*y(n-2) lds R20, _x32_1 ;load x(n-1) from RAM lds R21, _x32_1+1 sts _x32_2, r20 ;store x(n-2) to RAM sts _x32_2+1, R21 lds R20, _y31_1 ;load input parameter xx from stack lds R21, _y31_1+1 sts _x32_1, r20 ;store x(n-1) to RAM sts _x32_1+1, R21 lds R20, _y32_1 ;load y(n-1) from RAM lds R21, _y32_1+1 sts _y32_2, R20 ;store y(n-2) to RAM sts _y32_2+1, R21 sts _y32_1, r30 ;store new output as y(n-1) to RAM sts _y32_1+1, r31 ;*************************End of Second 2nd order IIR filter.************** ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G3 lds r21,_G3+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y3 lds r31,_y3+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 pop r21 ;restore parameter regs pop r20 #endasm end //**********************FILTER 4*************************** int filter4(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B411 ;load b1 from RAM lds R23, _B411+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B412 ;load b2 from RAM lds R23, _B412+1 lds R20, _x41_1 ;load x(n-1) from RAM lds R21, _x41_1+1 mult_acc ; b2*x(n-1) lds R22, _B413 ;load b3 from RAM lds R23, _B413+1 lds R20, _x41_2 ;load x(n-2) from RAM lds R21, _x41_2+1 mult_acc ; b3*x(n-2) lds R22, _A412 ;load -a2 from RAM lds R23, _A412+1 lds R20, _y41_1 ;load y(n-1) from RAM lds R21, _y41_1+1 mult_acc ; -a2*y(n-1) lds R22, _A413 ;load -a3 from RAM lds R23, _A413+1 lds R20, _y41_2 ;load y(n-2) from RAM lds R21, _y41_2+1 mult_acc ; -a3*y(n-2) lds R20, _x41_1 ;load x(n-1) from RAM lds R21, _x41_1+1 sts _x41_2, r20 ;store x(n-2) to RAM sts _x41_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x41_1, r20 ;store x(n-1) to RAM sts _x41_1+1, R21 lds R20, _y41_1 ;load y(n-1) from RAM lds R21, _y41_1+1 sts _y41_2, R20 ;store y(n-2) to RAM sts _y41_2+1, R21 sts _y41_1, r30 ;store new output as y(n-1) to RAM sts _y41_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B421 ;load b1 from RAM lds R23, _B421+1 lds R20, _y41_1 ;load input parameter xx from stack lds R21, _y41_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B422 ;load b2 from RAM lds R23, _B422+1 lds R20, _x42_1 ;load x(n-1) from RAM lds R21, _x42_1+1 mult_acc ; b2*x(n-1) lds R22, _B423 ;load b3 from RAM lds R23, _B423+1 lds R20, _x42_2 ;load x(n-2) from RAM lds R21, _x42_2+1 mult_acc ; b3*x(n-2) lds R22, _A422 ;load -a2 from RAM lds R23, _A422+1 lds R20, _y42_1 ;load y(n-1) from RAM lds R21, _y42_1+1 mult_acc ; -a2*y(n-1) lds R22, _A423 ;load -a3 from RAM lds R23, _A423+1 lds R20, _y42_2 ;load y(n-2) from RAM lds R21, _y42_2+1 mult_acc ; -a3*y(n-2) lds R20, _x42_1 ;load x(n-1) from RAM lds R21, _x42_1+1 sts _x42_2, r20 ;store x(n-2) to RAM sts _x42_2+1, R21 lds R20, _y41_1 ;load input parameter xx from stack lds R21, _y41_1+1 sts _x42_1, r20 ;store x(n-1) to RAM sts _x42_1+1, R21 lds R20, _y42_1 ;load y(n-1) from RAM lds R21, _y42_1+1 sts _y42_2, R20 ;store y(n-2) to RAM sts _y42_2+1, R21 sts _y42_1, r30 ;store new output as y(n-1) to RAM sts _y42_1+1, r31 ;*************************End of Second 2nd order IIR filter.************** ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G4 lds r21,_G4+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y4 lds r31,_y4+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 pop r21 ;restore parameter regs pop r20 #endasm end int filter5(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B511 ;load b1 from RAM lds R23, _B511+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B512 ;load b2 from RAM lds R23, _B512+1 lds R20, _x51_1 ;load x(n-1) from RAM lds R21, _x51_1+1 mult_acc ; b2*x(n-1) lds R22, _B513 ;load b3 from RAM lds R23, _B513+1 lds R20, _x51_2 ;load x(n-2) from RAM lds R21, _x51_2+1 mult_acc ; b3*x(n-2) lds R22, _A512 ;load -a2 from RAM lds R23, _A512+1 lds R20, _y51_1 ;load y(n-1) from RAM lds R21, _y51_1+1 mult_acc ; -a2*y(n-1) lds R22, _A513 ;load -a3 from RAM lds R23, _A513+1 lds R20, _y51_2 ;load y(n-2) from RAM lds R21, _y51_2+1 mult_acc ; -a3*y(n-2) lds R20, _x51_1 ;load x(n-1) from RAM lds R21, _x51_1+1 sts _x51_2, r20 ;store x(n-2) to RAM sts _x51_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x51_1, r20 ;store x(n-1) to RAM sts _x51_1+1, R21 lds R20, _y51_1 ;load y(n-1) from RAM lds R21, _y51_1+1 sts _y51_2, R20 ;store y(n-2) to RAM sts _y51_2+1, R21 sts _y51_1, r30 ;store new output as y(n-1) to RAM sts _y51_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B521 ;load b1 from RAM lds R23, _B521+1 lds R20, _y51_1 ;load input parameter xx from stack lds R21, _y51_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B522 ;load b2 from RAM lds R23, _B522+1 lds R20, _x52_1 ;load x(n-1) from RAM lds R21, _x52_1+1 mult_acc ; b2*x(n-1) lds R22, _B523 ;load b3 from RAM lds R23, _B523+1 lds R20, _x52_2 ;load x(n-2) from RAM lds R21, _x52_2+1 mult_acc ; b3*x(n-2) lds R22, _A522 ;load -a2 from RAM lds R23, _A522+1 lds R20, _y52_1 ;load y(n-1) from RAM lds R21, _y52_1+1 mult_acc ; -a2*y(n-1) lds R22, _A523 ;load -a3 from RAM lds R23, _A523+1 lds R20, _y52_2 ;load y(n-2) from RAM lds R21, _y52_2+1 mult_acc ; -a3*y(n-2) lds R20, _x52_1 ;load x(n-1) from RAM lds R21, _x52_1+1 sts _x52_2, r20 ;store x(n-2) to RAM sts _x52_2+1, R21 lds R20, _y51_1 ;load input parameter xx from stack lds R21, _y51_1+1 sts _x52_1, r20 ;store x(n-1) to RAM sts _x52_1+1, R21 lds R20, _y52_1 ;load y(n-1) from RAM lds R21, _y52_1+1 sts _y52_2, R20 ;store y(n-2) to RAM sts _y52_2+1, R21 sts _y52_1, r30 ;store new output as y(n-1) to RAM sts _y52_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G5 lds r21,_G5+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y5 lds r31,_y5+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end int filter6(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B611 ;load b1 from RAM lds R23, _B611+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B612 ;load b2 from RAM lds R23, _B612+1 lds R20, _x61_1 ;load x(n-1) from RAM lds R21, _x61_1+1 mult_acc ; b2*x(n-1) lds R22, _B613 ;load b3 from RAM lds R23, _B613+1 lds R20, _x61_2 ;load x(n-2) from RAM lds R21, _x61_2+1 mult_acc ; b3*x(n-2) lds R22, _A612 ;load -a2 from RAM lds R23, _A612+1 lds R20, _y61_1 ;load y(n-1) from RAM lds R21, _y61_1+1 mult_acc ; -a2*y(n-1) lds R22, _A613 ;load -a3 from RAM lds R23, _A613+1 lds R20, _y61_2 ;load y(n-2) from RAM lds R21, _y61_2+1 mult_acc ; -a3*y(n-2) lds R20, _x61_1 ;load x(n-1) from RAM lds R21, _x61_1+1 sts _x61_2, r20 ;store x(n-2) to RAM sts _x61_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x61_1, r20 ;store x(n-1) to RAM sts _x61_1+1, R21 lds R20, _y61_1 ;load y(n-1) from RAM lds R21, _y61_1+1 sts _y61_2, R20 ;store y(n-2) to RAM sts _y61_2+1, R21 sts _y61_1, r30 ;store new output as y(n-1) to RAM sts _y61_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B621 ;load b1 from RAM lds R23, _B621+1 lds R20, _y61_1 ;load input parameter xx from stack lds R21, _y61_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B622 ;load b2 from RAM lds R23, _B622+1 lds R20, _x62_1 ;load x(n-1) from RAM lds R21, _x62_1+1 mult_acc ; b2*x(n-1) lds R22, _B623 ;load b3 from RAM lds R23, _B623+1 lds R20, _x62_2 ;load x(n-2) from RAM lds R21, _x62_2+1 mult_acc ; b3*x(n-2) lds R22, _A622 ;load -a2 from RAM lds R23, _A622+1 lds R20, _y62_1 ;load y(n-1) from RAM lds R21, _y62_1+1 mult_acc ; -a2*y(n-1) lds R22, _A623 ;load -a3 from RAM lds R23, _A623+1 lds R20, _y62_2 ;load y(n-2) from RAM lds R21, _y62_2+1 mult_acc ; -a3*y(n-2) lds R20, _x62_1 ;load x(n-1) from RAM lds R21, _x62_1+1 sts _x62_2, r20 ;store x(n-2) to RAM sts _x62_2+1, R21 lds R20, _y61_1 ;load input parameter xx from stack lds R21, _y61_1+1 sts _x62_1, r20 ;store x(n-1) to RAM sts _x62_1+1, R21 lds R20, _y62_1 ;load y(n-1) from RAM lds R21, _y62_1+1 sts _y62_2, R20 ;store y(n-2) to RAM sts _y62_2+1, R21 sts _y62_1, r30 ;store new output as y(n-1) to RAM sts _y62_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G6 lds r21,_G6+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y6 lds r31,_y6+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end int filter7(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B711 ;load b1 from RAM lds R23, _B711+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B712 ;load b2 from RAM lds R23, _B712+1 lds R20, _x71_1 ;load x(n-1) from RAM lds R21, _x71_1+1 mult_acc ; b2*x(n-1) lds R22, _B713 ;load b3 from RAM lds R23, _B713+1 lds R20, _x71_2 ;load x(n-2) from RAM lds R21, _x71_2+1 mult_acc ; b3*x(n-2) lds R22, _A712 ;load -a2 from RAM lds R23, _A712+1 lds R20, _y71_1 ;load y(n-1) from RAM lds R21, _y71_1+1 mult_acc ; -a2*y(n-1) lds R22, _A713 ;load -a3 from RAM lds R23, _A713+1 lds R20, _y71_2 ;load y(n-2) from RAM lds R21, _y71_2+1 mult_acc ; -a3*y(n-2) lds R20, _x71_1 ;load x(n-1) from RAM lds R21, _x71_1+1 sts _x71_2, r20 ;store x(n-2) to RAM sts _x71_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x71_1, r20 ;store x(n-1) to RAM sts _x71_1+1, R21 lds R20, _y71_1 ;load y(n-1) from RAM lds R21, _y71_1+1 sts _y71_2, R20 ;store y(n-2) to RAM sts _y71_2+1, R21 sts _y71_1, r30 ;store new output as y(n-1) to RAM sts _y71_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B721 ;load b1 from RAM lds R23, _B721+1 lds R20, _y71_1 ;load input parameter xx from stack lds R21, _y71_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B722 ;load b2 from RAM lds R23, _B722+1 lds R20, _x72_1 ;load x(n-1) from RAM lds R21, _x72_1+1 mult_acc ; b2*x(n-1) lds R22, _B723 ;load b3 from RAM lds R23, _B723+1 lds R20, _x72_2 ;load x(n-2) from RAM lds R21, _x72_2+1 mult_acc ; b3*x(n-2) lds R22, _A722 ;load -a2 from RAM lds R23, _A722+1 lds R20, _y72_1 ;load y(n-1) from RAM lds R21, _y72_1+1 mult_acc ; -a2*y(n-1) lds R22, _A723 ;load -a3 from RAM lds R23, _A723+1 lds R20, _y72_2 ;load y(n-2) from RAM lds R21, _y72_2+1 mult_acc ; -a3*y(n-2) lds R20, _x72_1 ;load x(n-1) from RAM lds R21, _x72_1+1 sts _x72_2, r20 ;store x(n-2) to RAM sts _x72_2+1, R21 lds R20, _y71_1 ;load input parameter xx from stack lds R21, _y71_1+1 sts _x72_1, r20 ;store x(n-1) to RAM sts _x72_1+1, R21 lds R20, _y72_1 ;load y(n-1) from RAM lds R21, _y72_1+1 sts _y72_2, R20 ;store y(n-2) to RAM sts _y72_2+1, R21 sts _y72_1, r30 ;store new output as y(n-1) to RAM sts _y72_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G5 lds r21,_G5+1 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 sts _gainchk,r30 sts _gainchk+1,r31 ; this is just to test the output of second order filter. mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y7 lds r31,_y7+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end int filter8(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B811 ;load b1 from RAM lds R23, _B811+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B812 ;load b2 from RAM lds R23, _B812+1 lds R20, _x81_1 ;load x(n-1) from RAM lds R21, _x81_1+1 mult_acc ; b2*x(n-1) lds R22, _B813 ;load b3 from RAM lds R23, _B813+1 lds R20, _x81_2 ;load x(n-2) from RAM lds R21, _x81_2+1 mult_acc ; b3*x(n-2) lds R22, _A812 ;load -a2 from RAM lds R23, _A812+1 lds R20, _y81_1 ;load y(n-1) from RAM lds R21, _y81_1+1 mult_acc ; -a2*y(n-1) lds R22, _A813 ;load -a3 from RAM lds R23, _A813+1 lds R20, _y81_2 ;load y(n-2) from RAM lds R21, _y81_2+1 mult_acc ; -a3*y(n-2) lds R20, _x81_1 ;load x(n-1) from RAM lds R21, _x81_1+1 sts _x81_2, r20 ;store x(n-2) to RAM sts _x81_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x81_1, r20 ;store x(n-1) to RAM sts _x81_1+1, R21 lds R20, _y81_1 ;load y(n-1) from RAM lds R21, _y81_1+1 sts _y81_2, R20 ;store y(n-2) to RAM sts _y81_2+1, R21 sts _y81_1, r30 ;store new output as y(n-1) to RAM sts _y81_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B821 ;load b1 from RAM lds R23, _B821+1 lds R20, _y81_1 ;load input parameter xx from stack lds R21, _y81_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B822 ;load b2 from RAM lds R23, _B822+1 lds R20, _x82_1 ;load x(n-1) from RAM lds R21, _x82_1+1 mult_acc ; b2*x(n-1) lds R22, _B823 ;load b3 from RAM lds R23, _B823+1 lds R20, _x82_2 ;load x(n-2) from RAM lds R21, _x82_2+1 mult_acc ; b3*x(n-2) lds R22, _A822 ;load -a2 from RAM lds R23, _A822+1 lds R20, _y82_1 ;load y(n-1) from RAM lds R21, _y82_1+1 mult_acc ; -a2*y(n-1) lds R22, _A823 ;load -a3 from RAM lds R23, _A823+1 lds R20, _y82_2 ;load y(n-2) from RAM lds R21, _y82_2+1 mult_acc ; -a3*y(n-2) lds R20, _x82_1 ;load x(n-1) from RAM lds R21, _x82_1+1 sts _x82_2, r20 ;store x(n-2) to RAM sts _x82_2+1, R21 lds R20, _y81_1 ;load input parameter xx from stack lds R21, _y81_1+1 sts _x82_1, r20 ;store x(n-1) to RAM sts _x82_1+1, R21 lds R20, _y82_1 ;load y(n-1) from RAM lds R21, _y82_1+1 sts _y82_2, R20 ;store y(n-2) to RAM sts _y82_2+1, R21 sts _y82_1, r30 ;store new output as y(n-1) to RAM sts _y82_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G8 lds r21,_G8+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y8 lds r31,_y8+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end //// int filter9(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B911 ;load b1 from RAM lds R23, _B911+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B912 ;load b2 from RAM lds R23, _B912+1 lds R20, _x91_1 ;load x(n-1) from RAM lds R21, _x91_1+1 mult_acc ; b2*x(n-1) lds R22, _B913 ;load b3 from RAM lds R23, _B913+1 lds R20, _x91_2 ;load x(n-2) from RAM lds R21, _x91_2+1 mult_acc ; b3*x(n-2) lds R22, _A912 ;load -a2 from RAM lds R23, _A912+1 lds R20, _y91_1 ;load y(n-1) from RAM lds R21, _y91_1+1 mult_acc ; -a2*y(n-1) lds R22, _A913 ;load -a3 from RAM lds R23, _A913+1 lds R20, _y91_2 ;load y(n-2) from RAM lds R21, _y91_2+1 mult_acc ; -a3*y(n-2) lds R20, _x91_1 ;load x(n-1) from RAM lds R21, _x91_1+1 sts _x91_2, r20 ;store x(n-2) to RAM sts _x91_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x91_1, r20 ;store x(n-1) to RAM sts _x91_1+1, R21 lds R20, _y91_1 ;load y(n-1) from RAM lds R21, _y91_1+1 sts _y91_2, R20 ;store y(n-2) to RAM sts _y91_2+1, R21 sts _y91_1, r30 ;store new output as y(n-1) to RAM sts _y91_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B921 ;load b1 from RAM lds R23, _B921+1 lds R20, _y91_1 ;load input parameter xx from stack lds R21, _y91_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B922 ;load b2 from RAM lds R23, _B922+1 lds R20, _x92_1 ;load x(n-1) from RAM lds R21, _x92_1+1 mult_acc ; b2*x(n-1) lds R22, _B923 ;load b3 from RAM lds R23, _B923+1 lds R20, _x92_2 ;load x(n-2) from RAM lds R21, _x92_2+1 mult_acc ; b3*x(n-2) lds R22, _A922 ;load -a2 from RAM lds R23, _A922+1 lds R20, _y92_1 ;load y(n-1) from RAM lds R21, _y92_1+1 mult_acc ; -a2*y(n-1) lds R22, _A923 ;load -a3 from RAM lds R23, _A923+1 lds R20, _y92_2 ;load y(n-2) from RAM lds R21, _y92_2+1 mult_acc ; -a3*y(n-2) lds R20, _x92_1 ;load x(n-1) from RAM lds R21, _x92_1+1 sts _x92_2, r20 ;store x(n-2) to RAM sts _x92_2+1, R21 lds R20, _y91_1 ;load input parameter xx from stack lds R21, _y91_1+1 sts _x92_1, r20 ;store x(n-1) to RAM sts _x92_1+1, R21 lds R20, _y92_1 ;load y(n-1) from RAM lds R21, _y92_1+1 sts _y92_2, R20 ;store y(n-2) to RAM sts _y92_2+1, R21 sts _y92_1, r30 ;store new output as y(n-1) to RAM sts _y92_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G9 lds r21,_G9+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y9 lds r31,_y9+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end /// int filter10(int xx) // xx is the current input signal sample // returns the current filtered output sample begin #asm 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, _B1011 ;load b1 from RAM lds R23, _B1011+1 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 mult_acc ; b1*xx lds R22, _B1012 ;load b2 from RAM lds R23, _B1012+1 lds R20, _x101_1 ;load x(n-1) from RAM lds R21, _x101_1+1 mult_acc ; b2*x(n-1) lds R22, _B1013 ;load b3 from RAM lds R23, _B1013+1 lds R20, _x101_2 ;load x(n-2) from RAM lds R21, _x101_2+1 mult_acc ; b3*x(n-2) lds R22, _A1012 ;load -a2 from RAM lds R23, _A1012+1 lds R20, _y101_1 ;load y(n-1) from RAM lds R21, _y101_1+1 mult_acc ; -a2*y(n-1) lds R22, _A1013 ;load -a3 from RAM lds R23, _A1013+1 lds R20, _y101_2 ;load y(n-2) from RAM lds R21, _y101_2+1 mult_acc ; -a3*y(n-2) lds R20, _x101_1 ;load x(n-1) from RAM lds R21, _x101_1+1 sts _x101_2, r20 ;store x(n-2) to RAM sts _x101_2+1, R21 ld R20, Y ;load input parameter xx from stack ldd R21, Y+1 sts _x101_1, r20 ;store x(n-1) to RAM sts _x101_1+1, R21 lds R20, _y101_1 ;load y(n-1) from RAM lds R21, _y101_1+1 sts _y101_2, R20 ;store y(n-2) to RAM sts _y101_2+1, R21 sts _y101_1, r30 ;store new output as y(n-1) to RAM sts _y101_1+1, r31 ;*************************Second 2nd order IIR filter.************** lds R22, _B1021 ;load b1 from RAM lds R23, _B1021+1 lds R20, _y101_1 ;load input parameter xx from stack lds R21, _y101_1+1 clr r30 clr r31 mult_acc ; b1*xx lds R22, _B1022 ;load b2 from RAM lds R23, _B1022+1 lds R20, _x102_1 ;load x(n-1) from RAM lds R21, _x102_1+1 mult_acc ; b2*x(n-1) lds R22, _B1023 ;load b3 from RAM lds R23, _B1023+1 lds R20, _x102_2 ;load x(n-2) from RAM lds R21, _x102_2+1 mult_acc ; b3*x(n-2) lds R22, _A1022 ;load -a2 from RAM lds R23, _A1022+1 lds R20, _y102_1 ;load y(n-1) from RAM lds R21, _y102_1+1 mult_acc ; -a2*y(n-1) lds R22, _A1023 ;load -a3 from RAM lds R23, _A1023+1 lds R20, _y102_2 ;load y(n-2) from RAM lds R21, _y102_2+1 mult_acc ; -a3*y(n-2) lds R20, _x102_1 ;load x(n-1) from RAM lds R21, _x102_1+1 sts _x102_2, r20 ;store x(n-2) to RAM sts _x102_2+1, R21 lds R20, _y101_1 ;load input parameter xx from stack lds R21, _y101_1+1 sts _x102_1, r20 ;store x(n-1) to RAM sts _x102_1+1, R21 lds R20, _y102_1 ;load y(n-1) from RAM lds R21, _y102_1+1 sts _y102_2, R20 ;store y(n-2) to RAM sts _y102_2+1, R21 sts _y102_1, r30 ;store new output as y(n-1) to RAM sts _y102_1+1, r31 ; Adding new stuff ; multipling filter output w/ gain then ; squaring the output of multiplier mov r23,r31 mov r22,r30 clr r30 clr r31 lds r20, _G10 lds r21,_G10+1 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 mov r23,r31 mov r22,r30 ;Here multiplier with gain finishes ;Now start squaring y variable lds r30,_y10 lds r31,_y10+1 clr r24 muls r23,r23 add r31,r0 mul r22,r22 add r24,r0 adc r30,r1 adc r31,r27 mulsu r23,r22 add r30,r0 adc r31,r1 mulsu r23,r22 add r30,r0 adc r31,r1 ;*************************End of Second 2nd order IIR filter.************** pop r21 ;restore parameter regs pop r20 #endasm end //~~~~~~~~~~~~~~~~~~~~~~~note: need to include pragma warn int multfix(int a,int b) begin #asm push r20 push r21 LDD R22,Y+2 ;load lsb a LDD R23,Y+3 ; load msb a LD R20,Y ;load lsb b LDD R21,Y+1 ;load msb b 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 void main(void){ initialize(); // open files (if testing on workstation), while(1) begin /* // This is inital test for PWM signal to work. // before connecting a car. if (PINC == 0xFE){ minword =1; //go } else if (PINC == 0b11111101){ minword =2; // left } else if (PINC == 0b11111011){ minword =3; //right } else if (PINC == 0b11110111){ minword =4; //stop } else if (PINC == 0b11101111){ minword =5; //back } if(PINC != 0xFF){ carcontrol(); } */ if (new_smpl == 1) { get_sample(); // read in a word sample (assume framing is done by low level routine) //analyze(); if(WRDFND == 1) begin input = (((int)Ain) -102); //filter1(input); y2 = filter2(input); y3 = filter3(input); y4 = filter4(input); y5 = filter5(input); y6 = filter6(input); y7 = filter7(input); y8 = filter8(input); y9 = filter9(input); // y10 = filter10((int)Ain); samcnt++; if(samcnt == 125){ segcnt++; samcnt = 0; // Reset sample counter. j = 0; word[i][j++] = y1; word[i][j++] = y2; word[i][j++] = y3; word[i][j++] = y4; word[i][j++] = y5; word[i][j++] = y6; word[i][j++] = y7; word[i][j++] = y8; word[i][j++] = y9; word[i][j++] = y10; y1 = 0; y2 = 0; y3 = 0; y4 = 0; y5 = 0; y6 = 0; y7 = 0; y8 = 0; y9 = 0; y10 = 0; i++; } // samcnt if(segcnt == 16){ // only makes ADC 1/2 long instead full second (ifsegcnt 32) segcnt = 0; samcnt = 0; WRDFND = 0; i = 0; j = 0; for(m = 0; m < 10; m++){ for(l = 0; l < 16; l++){ fingerprint[k] = word[l][m]; k++; } } k = 0; lookup(); carcontrol(); // controls car printf("%d\r\n",minword); // To test which word is that } end // if statement new_smpl = 0; // Re initialize to 0 for next ADC conversion. } //new_smpl end // end while(1) }