#include <stdio.h>
+#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.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 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);
-std::pair<double, double> adjust_for_overtones(std::pair<double, double> base, double *in, unsigned num_samples);
-double bin_to_freq(double bin, unsigned num_samples);
-double freq_to_bin(double freq, 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()
{
- 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 ( ;; ) {
- 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);
- if (peak.first > 0.0)
- peak = adjust_for_overtones(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 1
-void read_chunk(int fd, double *in, unsigned num_samples)
+void read_chunk(int fd, short *in, unsigned num_samples)
{
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 != int(to_read * sizeof(short))) {
+ if (ret != int(num_samples * sizeof(short))) {
// 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
-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) {
- in[i] = cos(theta);
+ in[i] = 32768.0 * cos(theta);
theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
}
}
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 > 1 && in[best_bin - 1] > in[best_bin] && in[best_bin - 1] > in[best_bin - 2]) {
- --best_bin;
- } else if (best_bin < num_samples / 2 - 1 && in[best_bin + 1] > in[best_bin] && in[best_bin + 1] > in[best_bin + 2]) {
- ++best_bin;
- }
-
- break;
- }
- }
-
- if (best_bin == 0 || best_bin == num_samples / 2) {
- return std::make_pair(-1.0, 0.0);
- }
- 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);
-}
-
-// it's perhaps not ideal to _first_ find the peak and _then_ the harmonics --
-// ideally, one should find the set of all peaks and then determine the likely
-// base from that... something for later. :-)
-std::pair<double, double> adjust_for_overtones(std::pair<double, double> base, double *in, unsigned num_samples)
-{
- double mu = base.first, var = 1.0 / (base.second * base.second);
-
- //printf("Base at %.2f (amp=%5.2fdB)\n", base.first, base.second);
-
- for (unsigned i = 2; i < 10; ++i) {
- unsigned middle = unsigned(floor(freq_to_bin(base.first, num_samples) * i + 0.5));
- unsigned lower = middle - (i+1)/2, upper = middle + (i+1)/2;
-
- if (upper >= num_samples)
- upper = num_samples - 2;
-
- // printf("Searching in [%u,%u] = %f..%f\n", lower, upper, bin_to_freq(lower, num_samples), bin_to_freq(upper, num_samples));
-
- // search for a peak in this interval
- double best_harmonics_freq = -1.0;
- double best_harmonics_amp = -1.0;
- for (unsigned j = lower; j <= upper; ++j) {
- if (in[j] > in[j-1] && in[j] > in[j+1]) {
- std::pair<double, double> peak =
- interpolate_peak(in[j - 1],
- in[j],
- in[j + 1]);
-
- if (peak.second > best_harmonics_amp) {
- best_harmonics_freq = bin_to_freq(j + peak.first, num_samples);
- best_harmonics_amp = peak.second;
- }
- }
- }
-
- if (best_harmonics_amp <= 0.0)
- continue;
-
- //printf("Found overtone %u at %.2f (amp=%5.2fdB)\n", i, best_harmonics_freq,
- // best_harmonics_amp);
-
- double this_mu = best_harmonics_freq / double(i);
- double this_var = 1.0 / (best_harmonics_amp * best_harmonics_amp);
-
- double k = var / (var + this_var);
- mu = (1.0 - k) * mu + k * this_mu;
- var *= (1.0 - k);
- }
- return std::make_pair(mu, base.second);
-}
-
-double bin_to_freq(double bin, unsigned num_samples)
-{
- return bin * SAMPLE_RATE / double(num_samples);
-}
-double freq_to_bin(double freq, unsigned num_samples)
-{
- return freq * double(num_samples) / double(SAMPLE_RATE);
-}
-
-/*
- * 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" };
printf(" (%s %+.2f, %5.2fHz) [%5.2fdB] [", notes[best_away_ind].notename, best_away, freq, amp);
+ // coarse tuning
for (int i = -10; i <= 10; ++i) {
if (best_away >= (i-0.5) * 0.05 && best_away < (i+0.5) * 0.05) {
printf("#");
}
}
}
+ printf("] [");
+
+ // fine tuning
+ for (int i = -10; i <= 10; ++i) {
+ if (best_away >= (i-0.5) * 0.01 && best_away < (i+0.5) * 0.01) {
+ printf("#");
+ } else {
+ if (i == 0) {
+ printf("|");
+ } else {
+ printf("-");
+ }
+ }
+ }
printf("]\n");
}
#endif
projects / pitch / blobdiff