additional features for peak picking

Makefile

@@ -1,5 +1,5 @@
 
-all: kiss_fft_s kiss_fft_f kiss_fft_d
+all: kiss_fft_s kiss_fft_f kiss_fft_d freqpeak tones
 
 kiss_fft_s: kiss_fft.h kiss_fft.c
 	gcc -Wall -O3 -o kiss_fft_s -DFIXED_POINT -DFFT_UTIL kiss_fft.c -lm
@@ -9,12 +9,28 @@ kiss_fft_f: kiss_fft.h kiss_fft.c
 
 kiss_fft_d: kiss_fft.h kiss_fft.c
 	gcc -Wall -O3 -o kiss_fft_d -Dkiss_fft_scalar=double -DFFT_UTIL kiss_fft.c -lm
+	
+freqpeak: kiss_fft.h kiss_fft.c freqpeak.c
+	gcc -Wall -O3 -o freqpeak freqpeak.c kiss_fft.c -lm
 
+testsig: testsig.c
+	gcc -Wall -O3 -o testsig testsig.c -lm
+
+tones: tones.c
+	gcc -Wall -O3 -o tones tones.c -lm
+	
 clean:
-	rm -f kiss_fft_s kiss_fft_f kiss_fft_d *~ fftin.dat fftout.dat 
+	rm -f kiss_fft_s kiss_fft_f kiss_fft_d *~ fftin.dat fftout.dat \
+	freqpeak testsig tones
 
-test: clean all
+test: all
 	./test.oct
 
+speedtest: /dev/shm/junk kiss_fft_f
+	time ./kiss_fft_f < /dev/shm/junk > /dev/null
+
+/dev/shm/junk:
+	dd if=/dev/urandom bs=8192 count=1000 of=/dev/shm/junk
+
 tarball: clean
 	tar -czf kiss_fft.tar.gz .

freqpeak.c

@@ -0,0 +1,89 @@
+#include <stdio.h>
+#include <string.h>
+#include <memory.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <math.h>
+#include "kiss_fft.h"
+
+#define NFFT 1024
+int main(int argc, char ** argv)
+{
+    int k;
+    void * st;
+    float fs=44100;
+
+    short sampsin[2*NFFT];
+    float lmag2[NFFT/2];
+    float rmag2[NFFT/2];
+    int peakr=0,peakl=0;
+    int removedc=1;
+
+    kiss_fft_cpx cbuf[NFFT];
+    int nbufs=0;
+
+    st = kiss_fft_alloc(NFFT,0);
+
+    memset( lmag2 , 0 , sizeof(lmag2) );
+    memset( rmag2 , 0 , sizeof(rmag2) );
+
+    while ( fread( sampsin , sizeof(short) * 2*NFFT, 1 , stdin ) == 1 ) {
+        //perform two ffts in parallel by packing the channels into the real and imaginary
+        //
+        
+        for (k=0;k<NFFT;++k) {
+            //cbuf[k].r = 0;
+            //cbuf[k].i = 0;
+            cbuf[k].r = sampsin[2*k];
+            cbuf[k].i = sampsin[2*k+1];
+            //cbuf[k].i = sampsin[2*k+1];
+        }
+        
+        if (removedc){
+            float dcr=0,dci=0;
+            for (k=0;k<NFFT;++k){
+                dcr += cbuf[k].r;
+                dci += cbuf[k].i;
+            }
+            dcr /= NFFT;
+            dci /= NFFT;
+
+            for (k=0;k<NFFT;++k){
+                cbuf[k].r -= dcr;
+                cbuf[k].i -= dci;
+            }
+        }
+
+        kiss_fft( st , cbuf );
+
+        for (k=0;k<NFFT/2;++k) {
+            int k2 = (NFFT-k)%NFFT;
+            kiss_fft_cpx r,l;
+
+            r.r = (cbuf[k].r + cbuf[k2].r) * 0.5;
+            r.i = (cbuf[k].i - cbuf[k2].i) * 0.5;
+
+            l.r = (cbuf[k].i + cbuf[k2].i) * 0.5;
+            l.i = (cbuf[k].r - cbuf[k2].r) * 0.5;
+
+            rmag2[k] += r.r * r.r + r.i * r.i;
+            lmag2[k] += l.r * l.r + l.i * l.i;
+        }
+        ++nbufs;
+    }
+    free(st);
+
+    for (k=0;k<NFFT/2;++k) {
+        if (rmag2[peakr] < rmag2[k])
+            peakr = k;
+        if (lmag2[peakl] < lmag2[k])
+            peakl = k;
+    }
+
+    printf("%d buffers\n",nbufs);
+
+    printf("peak frequency R:%.3fdB @ %.1f Hz\n",10*log10(rmag2[peakr]) , peakr*fs/NFFT);
+    printf("               L:%.3fdB @ %.1f Hz\n",10*log10(lmag2[peakl]) , peakl*fs/NFFT);
+
+    return 0;
+}

kiss_fft.c

@@ -34,19 +34,29 @@ typedef struct {
 
 #ifdef FIXED_POINT
 
-#   define TWIDDLE_RANGE ( (1<<15) - 1 )
+#   define TWIDDLE_RANGE ( (1<<FIXED_POINT_FRAC_BITS) - 1 )
+
+    /* The fixed point versions of C_ADD and C_SUB divide by two
+     * after the add/sub.
+     * This is to prevent overflow.
+     * The cumulative effect is to multiply the sequence by 1/Nfft.
+     * */
 #   define C_ADD(x,a,b) \
      do{ (x).r = ( ( (a).r+(b).r +1) >> 1 );\
          (x).i = ( ( (a).i+(b).i +1) >> 1 ); }while(0)
 #   define C_SUB(x,a,b) \
      do{ (x).r = ( ( (a).r-(b).r +1) >> 1 );\
          (x).i = ( ( (a).i-(b).i +1) >> 1 ); }while(0)
-#   define C_MUL(m,a,b) \
-     do{ (m).r = ( ( (a).r*(b.r) - (a).i*(b).i)  + (1<<14) ) >> 15;\
-         (m).i = ( ( (a).r*(b.i) + (a).i*(b).r)  + (1<<14) ) >> 15;\
-         }while(0)
 
-#else // floating point
+    /*  We don't have to worry about overflow from multiplying by twiddle factors since they
+     *  all have unity magnitude.  Still need to shift away fractional bits after adding 1/2 for
+     *  rounding.
+     * */
+#   define C_MUL(m,a,b) \
+     do{ (m).r = ( ( (a).r*(b).r - (a).i*(b).i)  + (TWIDDLE_RANGE>>1) ) >> FIXED_POINT_FRAC_BITS;\
+         (m).i = ( ( (a).r*(b).i + (a).i*(b).r)  + (TWIDDLE_RANGE>>1) ) >> FIXED_POINT_FRAC_BITS;\
+         }while(0)
+#else // not FIXED_POINT
 
 #define C_ADD(x,a,b) \
     do{ (x).r = (a).r+(b).r;\
@@ -55,8 +65,8 @@ typedef struct {
     do{ (x).r = (a).r-(b).r;\
         (x).i = (a).i-(b).i;}while(0)
 #define C_MUL(m,a,b) \
-    do{ (m).r = (a).r*(b.r) - (a).i*(b).i;\
-        (m).i = (a).r*(b.i) + (a).i*(b).r; }while(0)
+    do{ (m).r = (a).r*(b).r - (a).i*(b).i;\
+        (m).i = (a).r*(b).i + (a).i*(b).r; }while(0)
 
 #endif
 
@@ -69,12 +79,15 @@ void make_twiddles(kiss_fft_cpx * twiddle,int nfft,int inverse_fft)
     int n;
     for (n=0;n< (nfft>>1);++n) {
 #ifdef FIXED_POINT
+        // TODO: find another way to calculate these without so much floating point math
         twiddle[n].r =  (kiss_fft_scalar)(TWIDDLE_RANGE*cos(2*pi*n/nfft)+.5);
         twiddle[n].i = -(kiss_fft_scalar)(TWIDDLE_RANGE*sin(2*pi*n/nfft)+.5);
 #else
         twiddle[n].r = cos(2*pi*n/nfft);
         twiddle[n].i = -sin(2*pi*n/nfft);
 #endif
+
+        // inverse fft uses complex conjugate twiddle factors
         if (inverse_fft)
             twiddle[n].i *= -1;
     }
@@ -126,6 +139,8 @@ void undo_bit_rev( kiss_fft_cpx * f, const int * swap_indices,int nswap)
     }
 }
 
+
+#define OPT1
 // the heart of the fft
 static
 void fft_work(int N,kiss_fft_cpx *f,const kiss_fft_cpx * twid,int twid_step)
@@ -134,17 +149,36 @@ void fft_work(int N,kiss_fft_cpx *f,const kiss_fft_cpx * twid,int twid_step)
     {   // declare automatic variables in here to reduce stack usage
         int n;
         kiss_fft_cpx csum,cdiff;
- 
-        for (n=0;n<N;++n) {
-            C_ADD(csum,  f[n] , f[N+n] );
-            C_SUB(cdiff,  f[n] , f[N+n] );
-            f[n]=csum;
-            C_MUL(f[N+n], cdiff, twid[n*twid_step] );
+#ifdef OPT1
+        const kiss_fft_cpx * cur_twid = twid+twid_step;
+        kiss_fft_cpx *f1 = f;
+        kiss_fft_cpx *f2 = f+N;
+
+        C_SUB( cdiff, *f1 , *f2 );
+        C_ADD( csum,  *f1 , *f2 );
+        *f1++ = csum;
+        *f2++ = cdiff;
+
+        for (n=1;n<N;++n)
+#else
+        const kiss_fft_cpx * cur_twid = twid;
+        kiss_fft_cpx *f1 = f;
+        kiss_fft_cpx *f2 = f+N;
+        for (n=0;n<N;++n)
+#endif
+        {
+            C_ADD( csum,  *f1 , *f2 );
+            C_SUB( cdiff, *f1 , *f2 );
+            *f1++ = csum;
+            C_MUL(*f2, cdiff, *cur_twid);
+            ++f2;
+            cur_twid += twid_step;
         }
     }
     if (N==1) return;
-    fft_work(N,f,twid,twid_step*2); 
+    //twid_step *= 2;
     fft_work(N,f+N,twid,twid_step*2);
+    fft_work(N,f,twid,twid_step*2); 
 }
 
 
@@ -183,23 +217,54 @@ int main(int argc,char ** argv)
     void *st;
     int nfft=1024;
     int inverse_fft=0;
+    int scale=0;
+    kiss_fft_cpx scaling;
 
     fprintf(stderr,"sizeof(kiss_fft_cpx) == %d\n",sizeof(kiss_fft_cpx) );
-
-    if (argc>1 && 0==strcmp("-i",argv[1])) {
-        inverse_fft = 1;
+    
+    // parse args
+    while (argc>1 && argv[1][0]=='-'){
+        if (0==strcmp("-i",argv[1])) {
+            inverse_fft = 1;
+        }else if(0==strcmp("-s",argv[1])) {
+            scale = 1;
+        }
         --argc;++argv;
     }
-
     if (argc>1) {
         nfft = atoi(argv[1]);
     }
 
+    // do we need to scale?
+    if (scale) {
+#ifdef FIXED_POINT
+        if ( ! inverse_fft ) {
+            scaling.r = nfft;
+            scaling.i = nfft;
+            fprintf(stderr,"scaling by %d\n",scaling.r);
+        }
+#else
+        if (inverse_fft ){
+            scaling.r = 1.0/nfft;
+            scaling.i = 1.0/nfft;
+            fprintf(stderr,"scaling by %e\n",scaling.r);
+        }
+#endif
+        else
+            scale = 0; // no need
+    }
+
     buf=(kiss_fft_cpx*)malloc( sizeof(kiss_fft_cpx) * nfft );
     st = kiss_fft_alloc( nfft ,inverse_fft );
-
     while ( fread( buf , sizeof(kiss_fft_cpx) , nfft , stdin ) > 0 ) {
         kiss_fft( st , buf );
+        if (scale) {
+            int k;
+            for (k=0;k<nfft;++k) {
+                buf[k].r = buf[k].r * scaling.r;
+                buf[k].i = buf[k].i * scaling.i;
+            }
+        }
         fwrite( buf , sizeof(kiss_fft_cpx) , nfft , stdout );
     }
 

kiss_fft.h

@@ -1,10 +1,13 @@
 #ifndef KISS_FFT_H
 #define KISS_FFT_H
 
-#ifndef kiss_fft_scalar
-# ifdef FIXED_POINT
-#   define kiss_fft_scalar short
-# else
+#ifdef FIXED_POINT
+
+# define kiss_fft_scalar short
+# define FIXED_POINT_FRAC_BITS 15
+
+#else
+# ifndef kiss_fft_scalar
 #   define kiss_fft_scalar float
 # endif
 #endif

test.oct

@@ -2,23 +2,23 @@
 
 # Compare the results of kiss_fft with those calculated by octave's fft routines
 
-nfft=1024;
+nfft=1024
 showdiff=0;
+use_inverse_fft = 1
 
+infile = 'fftin.dat'
+outfile = 'fftout.dat'
 for test_case =1:3
     printf( '====================================\n');
     if test_case == 1,
         prec = 'short'
         prog = './kiss_fft_s'
-        scale = nfft
     elseif test_case  == 2,
         prec = 'float'
         prog = './kiss_fft_f'
-        scale = 1
     else
         prec = 'double'
         prog = './kiss_fft_d'
-        scale = 1
     endif
 
     # create the input
@@ -26,24 +26,31 @@ for test_case =1:3
     input = floor( real(input)*32767 ) + j*floor(imag(input)*32767 );
 
     # write the input
-    f = fopen( "fftin.dat", "w", "native");
+    f = fopen( infile, "w", "native");
     to_write = [real(input);imag(input) ];
     fwrite(f,to_write,prec);
     fclose(f);
 
-    cmd = sprintf('%s %d < fftin.dat > fftout.dat',prog,nfft)
+    if use_inverse_fft,
+        cmd = sprintf('%s -s -i %d < %s > %s',prog,nfft,infile,outfile)
+        comp = ifft(input);
+    else        
+        cmd = sprintf('%s -s %d < %s > %s',prog,nfft,infile,outfile)
+        comp = fft(input);
+    endif
     system(cmd);
 
     # read the output
-    f = fopen("fftout.dat","r", "native");
+    f = fopen(outfile,"r", "native");
     from_read = fread(f,Inf,prec)';
-    output = (from_read(1:2:nfft*2) + j*from_read(2:2:nfft*2))        ;
-
-    comp = fft(input)/scale;
-    if test_case == 1,
-        comp = floor( real(comp) +.5) + j*floor( imag(comp) +.5);
-    endif
+    output = (from_read(1:2:nfft*2) + j*from_read(2:2:nfft*2));
 
+    diff = output - comp;
+    noise_pow =  sum( conj(diff).*diff );
+    sig_pow = sum( conj(comp).*comp );
+    snr = 10*log10( sig_pow / noise_pow )
+    avg_scale = mean(  abs(output) ./ abs(comp) )
+    var_scale = var(  abs(output) ./ abs(comp) )
     if showdiff,
         x=linspace(0,2*pi,nfft);
         figure(1);
@@ -53,12 +60,5 @@ for test_case =1:3
         figure(3);
         plot( x, abs(output-comp) );
         pause
-    else
-        diff = output - comp;
-        noise_pow =  sum( conj(diff).*diff );
-        sig_pow = sum( conj(comp).*comp );
-        snr = 10*log10( sig_pow / noise_pow )
-
-        avg_scale = mean(  abs(output) ./ abs(comp) )
     endif
 endfor

testsig.c

@@ -0,0 +1,32 @@
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+
+int usage()
+{
+    fprintf(stderr,"usage:testsig nsamps\n");
+    exit(1);
+    return 1;
+}
+
+double randphase()
+{
+    return (double)rand()*2*3.14159/RAND_MAX;
+}
+
+int main(int argc, char ** argv)
+{
+    float samps[2];
+    int nsamps;
+
+    if (argc != 2)
+        return usage();
+    nsamps = atoi( argv[1] );
+    
+    while (nsamps-- > 0) {
+        samps[0]=sin( randphase() );
+        samps[1]=sin( randphase() );
+        fwrite(samps,sizeof(samps),1,stdout);
+    }
+    return 0;
+}

tones.c

@@ -0,0 +1,35 @@
+
+#include <stdio.h>
+#include <string.h>
+#include <memory.h>
+#include <malloc.h>
+#include <stdio.h>
+#include <math.h>
+#include "kiss_fft.h"
+
+#define PI 3.14159
+
+int main(int argc, char ** argv)
+{
+    int k;
+    float fs=44100;
+
+    float fr=1,fl=300;
+
+    float th[2] = {0,0};
+    float thinc[2] = {2*PI*fr/fs,2*PI*fl/fs };
+
+    while (1){
+        for (k=0;k<2;++k){
+            short s;
+            th[k] += thinc[k];
+            if (th[k] > 2*PI){
+                th[k] -= 2*PI;
+            }
+            s=(short)32767*cos( th[k] );
+            fwrite(&s,sizeof(s),1,stdout);
+        }
+    }
+
+    return 0;
+}