Split the pitch logic into its own class.

[pitch] / pitch.cpp

diff --git a/pitch.cpp b/pitch.cpp
index be540560e1f309488f95f1141d6f3dc78eb48c25..933fef9aacc97896dc108dd2466ecbb85f23f8e3 100644 (file)
--- a/pitch.cpp
+++ b/pitch.cpp
@@ -9,6 +9,8 @@
 #include <sys/ioctl.h>
 #include <linux/soundcard.h>
 
 #include <sys/ioctl.h>
 #include <linux/soundcard.h>
 

+#include "pitchdetector.h"
+

 #define SAMPLE_RATE     22050
 #define FFT_LENGTH      4096     /* in samples */
 #define PAD_FACTOR      2        /* 1/pf of the FFT samples are real samples, the rest are padding */
 #define SAMPLE_RATE     22050
 #define FFT_LENGTH      4096     /* in samples */
 #define PAD_FACTOR      2        /* 1/pf of the FFT samples are real samples, the rest are padding */


@@ -23,41 +25,20 @@
 #define TUNING WELL_TEMPERED_GUITAR
 
 int get_dsp_fd();
 #define TUNING WELL_TEMPERED_GUITAR
 
 int get_dsp_fd();

-void read_chunk(int fd, double *in, unsigned num_samples);
-void apply_window(double *in, double *out, unsigned num_samples);
-std::pair<double, double> find_peak(double *in, unsigned num_samples);
-void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples);
-double bin_to_freq(double bin, unsigned num_samples);
-std::string freq_to_tonename(double freq);
-std::pair<double, double> interpolate_peak(double ym1, double y0, double y1);
+void read_chunk(int fd, short *in, unsigned num_samples);

 void print_spectrogram(double freq, double amp);
 void write_sine(int dsp_fd, double freq, unsigned num_samples);
 
 int main()
 {
 void print_spectrogram(double freq, double amp);
 void write_sine(int dsp_fd, double freq, unsigned num_samples);
 
 int main()
 {

-       double *in, *in_windowed;
-       std::complex<double> *out;
-       double *bins;
-       fftw_plan p;
-
-       // allocate memory
-       in = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH / PAD_FACTOR));
-       in_windowed = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH));
-       out = reinterpret_cast<std::complex<double> *> (fftw_malloc(sizeof(std::complex<double>) * (FFT_LENGTH / 2 + 1)));
-       bins = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH / 2 + 1));
-
-       memset(in, 0, sizeof(double) * FFT_LENGTH / PAD_FACTOR);
-
-       // init FFTW
-       p = fftw_plan_dft_r2c_1d(FFT_LENGTH, in_windowed, reinterpret_cast<fftw_complex *> (out), FFTW_ESTIMATE);
+       PitchDetector pd(SAMPLE_RATE, FFT_LENGTH, PAD_FACTOR, OVERLAP);

        
        int fd = get_dsp_fd();
        for ( ;; ) {
        
        int fd = get_dsp_fd();
        for ( ;; ) {

-               read_chunk(fd, in, FFT_LENGTH);
-               apply_window(in, in_windowed, FFT_LENGTH);
-               fftw_execute(p);
-               find_peak_magnitudes(out, bins, FFT_LENGTH);
-               std::pair<double, double> peak = find_peak(bins, FFT_LENGTH);
+               short buf[FFT_LENGTH / PAD_FACTOR / OVERLAP];
+
+               read_chunk(fd, buf, FFT_LENGTH / PAD_FACTOR / OVERLAP);
+               std::pair<double, double> peak = pd.detect_pitch(buf);

 
                if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) {
 #if TUNING == WELL_TEMPERED_GUITAR
 
                if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) {
 #if TUNING == WELL_TEMPERED_GUITAR


@@ -87,45 +68,43 @@ int get_dsp_fd()
        int chan = 1;
        ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan);
        
        int chan = 1;
        ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan);
        

-       int rate = 22050;
+       int rate = SAMPLE_RATE;

        ioctl(fd, SOUND_PCM_WRITE_RATE, &rate);
 
        ioctl(fd, SOUND_PCM_WRITE_RATE, &rate);
 

+       int max_fragments = 2;
+       int frag_shift = ffs(FFT_LENGTH / OVERLAP) - 1;
+       int fragments = (max_fragments << 16) | frag_shift;
+        ioctl(fd, SNDCTL_DSP_SETFRAGMENT, &fragments);
+       

        ioctl(3, SNDCTL_DSP_SYNC, 0);
        
        return fd;
 }
 
 #if 1
        ioctl(3, SNDCTL_DSP_SYNC, 0);
        
        return fd;
 }
 
 #if 1

-void read_chunk(int fd, double *in, unsigned num_samples)
+void read_chunk(int fd, short *in, unsigned num_samples)

 {
        int ret;
 {
        int ret;

-       unsigned to_read = num_samples / PAD_FACTOR / OVERLAP;
-       short buf[to_read];

 
 

-       memmove(in, in + to_read, (num_samples / PAD_FACTOR - to_read) * sizeof(double));
-       
-       ret = read(fd, buf, to_read * sizeof(short));
+       ret = read(fd, in, num_samples * sizeof(short));

        if (ret == 0) {
                printf("EOF\n");
                exit(0);
        }
 
        if (ret == 0) {
                printf("EOF\n");
                exit(0);
        }
 

-       if (ret != int(to_read * sizeof(short))) {
+       if (ret != int(num_samples * sizeof(short))) {

                // blah
                perror("read");
                exit(1);
        }
                // blah
                perror("read");
                exit(1);
        }

-       
-       for (unsigned i = 0; i < to_read; ++i)
-               in[i + (num_samples / PAD_FACTOR - to_read)] = double(buf[i]);

 }
 #else
 // make a pure 440hz sine for testing
 }
 #else
 // make a pure 440hz sine for testing

-void read_chunk(int fd, double *in, unsigned num_samples)
+void read_chunk(int fd, short *in, unsigned num_samples)

 {
        static double theta = 0.0;
        for (unsigned i = 0; i < num_samples; ++i) {
 {
        static double theta = 0.0;
        for (unsigned i = 0; i < num_samples; ++i) {

-               in[i] = cos(theta);
+               in[i] = 32768.0 * cos(theta);

                theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
        }
 }
                theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
        }
 }


@@ -144,160 +123,6 @@ void write_sine(int dsp_fd, double freq, unsigned num_samples)
        write(dsp_fd, buf, num_samples * sizeof(short));
 }
 
        write(dsp_fd, buf, num_samples * sizeof(short));
 }
 

-// Apply a standard Hamming window to our input data.
-void apply_window(double *in, double *out, unsigned num_samples)
-{
-       static double *win_data;
-       static unsigned win_len;
-       static bool win_inited = false;
-
-       // Initialize the window for the first time
-       if (!win_inited) {
-               win_len = num_samples / PAD_FACTOR;
-               win_data = new double[win_len];
-
-               for (unsigned i = 0; i < win_len; ++i) {
-                       win_data[i] = 0.54 - 0.46 * cos(2.0 * M_PI * double(i) / double(win_len - 1));
-               }
-
-               win_inited = true;
-       }
-
-       assert(win_len == num_samples / PAD_FACTOR);
-       
-       for (unsigned i = 0; i < win_len; ++i) {
-               out[i] = in[i] * win_data[i];
-       }
-       for (unsigned i = win_len; i < num_samples; ++i) {
-               out[i] = 0.0;
-       }
-}
-
-void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples)
-{
-       for (unsigned i = 0; i < num_samples / 2 + 1; ++i)
-               out[i] = abs(in[i]);
-}
-
-std::pair<double, double> find_peak(double *in, unsigned num_samples)
-{
-       double best_peak = in[0];
-       unsigned best_bin = 0;
-
-       for (unsigned i = 1; i < num_samples / 2 + 1; ++i) {
-               if (in[i] > best_peak) {
-                       best_peak = in[i];
-                       best_bin = i;
-               }
-       }
-       
-       if (best_bin == 0 || best_bin == num_samples / 2) {
-               return std::make_pair(-1.0, 0.0);
-       }
-
-#if 0
-       printf("undertone strength: %+4.2f %+4.2f %+4.2f [%+4.2f] %+4.2f %+4.2f %+4.2f\n",
-               20.0 * log10(in[best_bin*4] / FFT_LENGTH),
-               20.0 * log10(in[best_bin*3] / FFT_LENGTH),
-               20.0 * log10(in[best_bin*2] / FFT_LENGTH),
-               20.0 * log10(in[best_bin] / FFT_LENGTH),
-               20.0 * log10(in[best_bin/2] / FFT_LENGTH),
-               20.0 * log10(in[best_bin/3] / FFT_LENGTH),
-               20.0 * log10(in[best_bin/4] / FFT_LENGTH));
-#endif
-
-       // see if we might have hit an overtone (set a limit of 5dB)
-       for (unsigned i = 4; i >= 1; --i) {
-               if (best_bin != best_bin / i &&
-                   20.0 * log10(in[best_bin] / in[best_bin / i]) < 5.0f) {
-#if 0
-                       printf("Overtone of degree %u!\n", i);
-#endif
-                       best_bin /= i;
-
-                       // consider sliding one bin up or down
-                       if (best_bin > 0 && in[best_bin - 1] > in[best_bin] && in[best_bin - 1] > in[best_bin - 2]) {
-                               --best_bin;
-                       } else if (best_bin < num_samples / 2 && in[best_bin + 1] > in[best_bin] && in[best_bin + 1] > in[best_bin + 2]) {
-                               ++best_bin;
-                       }
-                          
-                       break;
-               }
-       }
-
-       std::pair<double, double> peak = 
-               interpolate_peak(in[best_bin - 1],
-                                in[best_bin],
-                                in[best_bin + 1]);
-               
-       return std::make_pair(bin_to_freq(double(best_bin) + peak.first, num_samples), peak.second);
-}
-
-double bin_to_freq(double bin, unsigned num_samples)
-{
-       return bin * SAMPLE_RATE / double(num_samples);
-}
-
-/*
- * Given three bins, find the interpolated real peak based
- * on their magnitudes. To do this, we execute the following
- * plan:
- * 
- * Fit a polynomial of the form ax^2 + bx + c = 0 to the data
- * we have. Maple readily yields our coefficients, assuming
- * that we have the values at x=-1, x=0 and x=1:
- *
- * > f := x -> a*x^2 + b*x + c;                                
- * 
- *                               2
- *           f := x -> a x  + b x + c
- * 
- * > cf := solve({ f(-1) = ym1, f(0) = y0, f(1) = y1 }, { a, b, c });
- * 
- *                            y1    ym1       y1    ym1
- *        cf := {c = y0, b = ---- - ---, a = ---- + --- - y0}
- *                            2      2        2      2
- *
- * Now let's find the maximum point for the polynomial (it has to be
- * a maximum, since y0 is the greatest value):
- *
- * > xmax := solve(subs(cf, diff(f(x), x)) = 0, x);
- * 
- *                                -y1 + ym1
- *                   xmax := -------------------
- *                           2 (y1 + ym1 - 2 y0)
- *
- * We could go further and insert {fmax,a,b,c} into the original
- * polynomial, but it just gets hairy. We calculate a, b and c separately
- * instead.
- *
- * http://www-ccrma.stanford.edu/~jos/parshl/Peak_Detection_Steps_3.html
- * claims that detection is almost twice as good when using dB scale instead
- * of linear scale for the input values, so we use that. (As a tiny bonus,
- * we get back dB scale from the function.)
- */
-std::pair<double, double> interpolate_peak(double ym1, double y0, double y1)
-{
-       ym1 = log10(ym1);
-       y0 = log10(y0);
-       y1 = log10(y1);
-
-#if 0
-       assert(y0 >= y1);
-       assert(y0 >= ym1);
-#endif
-       
-       double a = 0.5 * y1 + 0.5 * ym1 - y0;
-       double b = 0.5 * y1 - 0.5 * ym1;
-       double c = y0;
-       
-       double xmax = (ym1 - y1) / (2.0 * (y1 + ym1 - 2.0 * y0));
-       double ymax = 20.0 * (a * xmax * xmax + b * xmax + c) - 90.0;
-
-       return std::make_pair(xmax, ymax);
-}
-

 std::string freq_to_tonename(double freq)
 {
        std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
 std::string freq_to_tonename(double freq)
 {
        std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };