/* cQtrans.c 4/91 brown@nadia second version - with real and imag input from fft usage: cQtrans -flags -kKernel.fft < infile.frm.fft(wRflag) > outfile kernel numbers generated with genkern; fft -R 256 256 gives kernel.fft infile is the fft w R flag of file for which want the const Q trans outfile is the const Q trans 4/18/91 add window normalization 5/21/91 add phase option 5/23/91 add frame by frame phase diff compared to diff corresp to that freq bin; Some of the Options: 'f' cutoff for kernels 'P' phaseOut calc phase for output, no mag out 'R' deriv take frame by frame derivative (usually of phase ) 'a' avWct take abs val of phase diff if input 0 here; else drop values < avWct ; purpose is to pick out -PI to PI changes in phase which occur once per period. 'F' decRate print phase diffs to stderr for this decimation factor 's' ppdts print phase diffs to screen for above decimation factor 'D' ppdtf write phase diff of 1st frm plus theoretical to dptfil for plotting with drawplot */ #include #include #include #include /* definition of OPEN_MAX */ #define PI 3.1415927 #define NMAXFREQS 2048 #define MAXBYTES 1000000 #define NCHNLSFFT 128 /* 128 real AND 128 imag */ #define NCQCMP 60 #define MAXFRMSZ 1024 #define MINHZ 174.6 /* F3 = 7.05 midi=53 */ #define SRATE 11025. float infloat[MAXBYTES]; float outfloat[MAXBYTES]; float kernbuf[MAXBYTES]; float two1twlfth; float rlsm[MAXFRMSZ]; float imsm[MAXFRMSZ]; float phasebuf[MAXBYTES]; float *phasebufp; char kernfil[20]; char defltkern[20] = "kernR.256.60"; char dptfil[20] = "dpt.60" ; int ofp3; /* FILE *fopen(), *ofp2; */ main(argc,argv) int argc; char **argv; { /* begin main */ char *cp; int flag, debug=0, debug2=0 ; int k, comp, n, nr, nk, nfltsout, ncQcmp = NCQCMP, tfrm; int cQmax = ncQcmp, phaseOut=0, deriv=0, decRate=0; int ppdtf=0, calcPV=0, ppdts=0; int rdkern = 0, ifp, maxtfrm, nchnlsfft=NCHNLSFFT, nfft,cmpout=0; float *kernelp, *outfloatp, *infloatp, *inforfrmp, realsum,imsum; float dbgout, cutoff = 0., *rlsmp = rlsm, *imsmp = imsm, maxcmp=0. ; float *testp, avWct=100., two1twlfth = pow(2.,(1./12.)); argc--; argv++; /* align onto command args */ while ((cp = *argv) && *cp++ =='-' && (flag = *cp++)) { switch (flag){ case 'd': sscanf(cp, "%d", &debug); break; case 'w': sscanf(cp, "%s", kernfil); rdkern = 1; /* for kernel file other than default*/ break; case 'e': sscanf(cp, "%d", &debug2); break; case 'P': sscanf(cp, "%d", &phaseOut); break; /* calc phase for output, no mag out*/ case 'R': sscanf(cp, "%d", &deriv); break; /* take derivative of phase */ case 'a': sscanf(cp, "%f", &avWct);/*abs val of phase diff if input 0 here; else drop values < avWct */ break; case 'f': sscanf(cp, "%f", &cutoff); break; case 'F': sscanf(cp, "%d", &decRate); break; /* decimation rate*/ case 's': sscanf(cp, "%d", &ppdts); break; /*print phase diffs to screen for above decimation factor */ case 'D': sscanf(cp, "%d", &ppdtf); break; /* write phase diff of 1st frm */ /* plus theoretical to file*/ case 'V': sscanf(cp, "%d", &calcPV); break; default: fprintf(stderr, "unknown option\n"); break; } argc--; argv++; } if (nchnlsfft > MAXFRMSZ) merde("nchnlsfft is > MAXFRMSZ"); if (rdkern){ /* read in kernel file rather than use default */ if((ifp = open(kernfil,O_RDONLY)) < 0) merde("Can't open kernfil"); fprintf(stderr, "reading kernels from file '%s'.\n",kernfil); } else{ if((ifp = open(defltkern,O_RDONLY)) < 0) merde("Can't open default kernfil"); /*fprintf(stderr, "Reading kernels from file '%s'",defltkern); */ } if (((nk = (read(ifp, kernbuf, MAXBYTES)))) <= 0 ) merde("Error reading in kernels"); /*else fprintf(stderr, "( %d bytes from kernel.)\n", nk); */ if ((nr = read(0, infloat, MAXBYTES)) <= 0) merde("Can't read input fft"); else fprintf(stderr,"CQTRANS: Read %d bytes = %d floatsams from stdin\n", nr, nr>>2); fprintf(stderr,"Writing drawplot file to '%s'\n",dptfil); if(ppdtf || calcPV) if ((ofp3 = creat(dptfil, 644)) < 0 ) /* if ((ofp = fopen(dptfil, "r+")) == NULL) */ merde("Can't open draw plot file "); if(cutoff > 0.) fprintf(stderr,"Cutoff for kernels=%.2f\n",cutoff); maxtfrm = (nr>>3)/nchnlsfft; /*no time frames = no smps in/(no fft chnls*2 for Re and Im)*/ nfltsout = maxtfrm*cQmax ; /*no out samps = no time frms*no cQ comps*/ infloatp = infloat; kernelp = kernbuf; outfloatp = outfloat; inforfrmp = infloat; /* pointer to keep track of beg of frm of infile (the fft) */ fprintf(stderr,"%d time frames; %d cQ comps; %d input fft chnls Re/Im\n", maxtfrm, cQmax, nchnlsfft); if(debug) fprintf(stderr,"\t\tkernel values"); /***************** CALC OF CQ CMPS ********************************/ for (tfrm = 0; tfrm < maxtfrm;++tfrm){/*fr ech tm frm of fft*/ kernelp = kernbuf; /* reset kernels to beg of buffer */ rlsmp = rlsm; imsmp = imsm; /* reset for use with phaseOut */ maxcmp = 0.; /* for phase out */ for(ncQcmp=0;ncQcmp 0. ? *kernelp : - *kernelp) < cutoff) *kernelp = 0. ;/* drop small values of kernels*/ if( !tfrm && debug && (*kernelp > 0. ? *kernelp : - *kernelp) >= cutoff) fprintf(stderr,"%.3f\t", *kernelp); realsum += (*kernelp) * (*infloatp++); imsum += (*kernelp) * (*infloatp++); kernelp += 2; /*skip kern imag comp as 0*/ } /* end calq of a cq comp for this fft frame */ /* A */ if (phaseOut){ /* save real and imag parts for phase calc */ *rlsmp++ = realsum; *imsmp++ = imsum; } *outfloatp++ = pow((pow(realsum,2.) + pow(imsum,2.)),.5); if (phaseOut) /* keep track of max val */ if (*(outfloatp - 1) > maxcmp) maxcmp = *(outfloatp - 1); } /* end calc of the cq cmps for this time frame */ /**** Calc phase after have calc the magnitudes for the entire frame as need * **** maxval for the frame to set phase = 0 for small magnitudes ************/ if(phaseOut){ /* get phases rather than magnitudes */ calcphase(outfloatp, maxcmp,ncQcmp, debug2); } inforfrmp += 2*nchnlsfft; /*next frm of input fft; 2 for Re,then Im*/ } /* end this time frame */ /******* END CALC OF CQ COMPS ****************************************/ if (deriv){/* frm by frame deriv of the whole buffer of phase values*/ derivative(ncQcmp, &tfrm, avWct,decRate,nchnlsfft,ppdtf,ppdts,calcPV); } if ((nr = write(1, outfloat, nfltsout*4)) <= 0) merde("Problem writing output file"); else fprintf(stderr,"Wrote %d bytes = %d floatsams to stdout\n", nr, nr>>2); } /* end main */ merde(s) char *s; { fprintf(stderr,"MERDE, ALORS! %s\n",s); exit(1); } /* MISC PRINTING REMOVED */ /* if( !tfrm && debug && (*kernelp > 0. ? *kernelp : - *kernelp) >= cutoff) fprintf(stderr,"ncqcmp=%d nfftchnl=%d *kernelp=%.3f cutoff=%.2f\n", ncQcmp, nfft, *kernelp, cutoff); */ /* A if (cmpout == 1) put this option back in if want cmps out *outfloatp++ = realsum; else if (cmpout == 2) *outfloatp++ = imsum; */ /* case 'c': sscanf(cp, "%d", &cmpout); break; 1 for real out, 2 for imag out */ calcphase(otfltp, maxmag,nchnlscq,debug) /*outfloatp, maxcmp */ float *otfltp, maxmag ; int nchnlscq, debug; { float *rlsmp = rlsm, *imsmp = imsm, *magp, ratio, magRat, ctoffPh=.05; int ncq; otfltp -= nchnlscq; magp = otfltp; for(ncq=0 ; ncq < nchnlscq; ++ncq){ ratio = *imsmp / *rlsmp ; /* imag / real */ magRat = (ratio > 0 ? ratio : -ratio) ; if (*magp++ < ctoffPh * maxmag) *otfltp++ = 0. ; else if (magRat < .01 ){ /* imag < .01 real so 0 or PI*/ if(*rlsmp > 0.) *otfltp++ = 0. ; /* both comp + or (real + and imag - )*/ else if (*rlsmp < 0.) *otfltp++ = PI ; /* both - or (real - and imag + )*/ } else *otfltp++ = atan2(*imsmp , *rlsmp); /* if (debug && (!prntFrNo || (nfft == prntFrNo) ) ){ */ if (debug ){ fprintf(stderr,"%d\t%.1f \t%.1f \t%.1f \t%.2f \t=%.1fPI\t(%.1f deg)\n" ,ncq,*rlsmp, *imsmp, *(magp-1),*(otfltp-1), *(otfltp-1)/PI, *(otfltp-1) * 180./PI ); } ++rlsmp; ++imsmp; } /* end cq phases for this time frame */ if(debug) fprintf(stderr,"\n\n"); } /* frame by frame deriv of the whole buffer */ /*derivative(ncQcmp, &tfrm, avWct,decRate,nchnlsfft,ppdtf,ppdts,calcPV) */ derivative(nc, tfp,avWct,decRate,nchannelsfft, ppdtf, ppdts,calcPV) int nc, *tfp,decRate,nchannelsfft, ppdtf,ppdts, calcPV; float avWct; { float *ofptr = outfloat,freq, ph1, ph2 ,pdpus,freqCorr,crtdFreq; float pdpusF,facteur3=SRATE/(2.* PI); int nn, jj, nw; *tfp -= 1; /* one less time frame for deriv */ two1twlfth = pow(2.,(1./12.)); for(nn = 0; nn < *tfp; ++nn){ for(jj = 0; jj < nc; ++jj){ *ofptr = *(ofptr + nc) - *ofptr ; /* diff of corresp terms in successive frames */ if(avWct < 99.9){ /* take abs value if avWct < default=100. */ *ofptr = (*ofptr > 0. ? *ofptr : -*ofptr); /* take abs val*/ *ofptr = (*ofptr > avWct ? *ofptr : 0.); /*set 0 if <= (value of avWct) -> only the steps from -PI to PI which occur once/period */ } ++ofptr; } } if(avWct < 99.9) fprintf(stderr,"Taking abs val of deriv with 0 if < %.3f\n", avWct); if(decRate){/*decimation rate*/ /* print phase diffs and compare to phase diff predicted by center freqs*/ ofptr -= nc ; /* back one frame */ if(ppdts && !calcPV) fprintf(stderr, "Chnl\tfreq\tCnhg.phase.pred\tChng.phase.meas\tdiff\tfreq\n"); freq = MINHZ; /* if (ppdtf || calcPV){ /* write a drawplot file */ /* fprintf(ofp2, "0. 5500. -3.2 3.2 \n 3 \n 60"); */ /* } */ if(ppdtf || calcPV) phasebufp = phasebuf; for(jj = 0; jj < nc; ++jj){ ph1 = 2*PI* decRate*freq/SRATE ; /* theoretical */ ph2 = *ofptr++ ; /* measured */ if(ppdts && !calcPV) fprintf(stderr,"%d\t%.2f\t%.4f\t\t%.4f\t\t%.4f\t%.2f\n", jj, freq, ph1, ph2, ph1 - ph2, facteur3*ph2 ); while(ph2 > PI) /* unwrap */ ph2 -= 2*PI; while (ph2 < -PI) ph2 += 2*PI; if (ppdtf ) { *phasebufp = ph1; *(phasebufp + nc) = ph2 -1. ; /* -1 if for drawplot filter */ *(phasebufp + 2* nc ) = (ph2 < .000001 ? -2. :(ph1 - ph2)-2.); ++phasebufp; } else if(calcPV){ pdpus = ph2/(2*PI*decRate/nchannelsfft); pdpusF = pdpus - (float)((int)pdpus); freqCorr = pdpusF * SRATE / nchannelsfft; crtdFreq = freq + freqCorr; *phasebufp = freq; *(phasebufp + nc) = freqCorr - 1.; *(phasebufp + 2* nc ) =crtdFreq - 2.; if(ppdts) fprintf(stderr,"%d\t%.2f\t\t%.4f\t\t%.2f\t%.2f\n", jj, freq, freqCorr, crtdFreq,*(phasebufp + 2*nc) ); ++phasebufp; } freq *= two1twlfth; } } nw = (write(ofp3, phasebuf, 12 * nc)) ; /* 3 output frames cqcmp long */ } writdrwplt() { } /* can write a drawplot file directly using phasebuf[], but will have to do log plot if use freq on x (would be nice probably) */