From: Steinar H. Gunderson Date: Mon, 11 Jul 2005 10:11:14 +0000 (+0000) Subject: initial import X-Git-Url: https://git.sesse.net/?p=pitch;a=commitdiff_plain;h=0564163ef86509fe45a4a4fdd5004be2bc3b0e6c initial import (automatically generated log message) --- 0564163ef86509fe45a4a4fdd5004be2bc3b0e6c diff --git a/pitch.cpp b/pitch.cpp new file mode 100644 index 0000000..8080748 --- /dev/null +++ b/pitch.cpp @@ -0,0 +1,334 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 OVERLAP 4 /* 1/ol samples will be replaced in the buffer every frame. Should be + * a multiple of 2 for the Hamming window (see + * http://www-ccrma.stanford.edu/~jos/parshl/Choice_Hop_Size.html). + */ + +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 find_peak(double *in, unsigned num_samples); +void find_peak_magnitudes(std::complex *in, double *out, unsigned num_samples); +double bin_to_freq(double bin, unsigned num_samples); +std::string freq_to_tonename(double freq); +std::pair interpolate_peak(double ym1, double y0, double y1); +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 *out; + double *bins; + fftw_plan p; + + // allocate memory + in = reinterpret_cast (fftw_malloc(sizeof(double) * FFT_LENGTH / PAD_FACTOR)); + in_windowed = reinterpret_cast (fftw_malloc(sizeof(double) * FFT_LENGTH)); + out = reinterpret_cast *> (fftw_malloc(sizeof(std::complex) * (FFT_LENGTH / 2 + 1))); + bins = reinterpret_cast (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 (out), FFTW_ESTIMATE); + + 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 peak = find_peak(bins, FFT_LENGTH); + + if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) { + printf("............\n"); + } else { + print_spectrogram(peak.first, peak.second - log10(FFT_LENGTH)); + } + } +} + +int get_dsp_fd() +{ + int fd = open("/dev/dsp", O_RDWR); + if (fd == -1) { + perror("/dev/dsp"); + exit(1); + } + + ioctl(3, SNDCTL_DSP_RESET, 0); + + int fmt = AFMT_S16_LE; // FIXME + ioctl(fd, SNDCTL_DSP_SETFMT, &fmt); + + int chan = 1; + ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan); + + int rate = 22050; + ioctl(fd, SOUND_PCM_WRITE_RATE, &rate); + + ioctl(3, SNDCTL_DSP_SYNC, 0); + + return fd; +} + +#if 1 +void read_chunk(int fd, double *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)); + if (ret == 0) { + printf("EOF\n"); + exit(0); + } + + if (ret != int(to_read * 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) +{ + static double theta = 0.0; + for (unsigned i = 0; i < num_samples; ++i) { + in[i] = cos(theta); + theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE); + } +} +#endif + +void write_sine(int dsp_fd, double freq, unsigned num_samples) +{ + static double theta = 0.0; + short buf[num_samples]; + + for (unsigned i = 0; i < num_samples; ++i) { + buf[i] = short(cos(theta) * 16384.0); + theta += 2.0 * M_PI * freq / 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 *in, double *out, unsigned num_samples) +{ + for (unsigned i = 0; i < num_samples / 2 + 1; ++i) + out[i] = abs(in[i]); +} + +std::pair 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 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 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" }; + double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0; + int hnai = int(floor(half_notes_away + 0.5)); + int octave = (hnai + 48) / 12; + + char buf[256]; + sprintf(buf, "%s%d + %.2f [%d]", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai, hnai); + return buf; +} + +void print_spectrogram(double freq, double amp) +{ + std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" }; + double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0; + int hnai = int(floor(half_notes_away + 0.5)); + int octave = (hnai + 48) / 12; + + for (int i = 0; i < 12; ++i) + if (i == ((hnai % 12) + 12) % 12) + printf("#"); + else + printf("."); + + printf(" (%-2s%d %+.2f, %5.2fHz) [%5.2fdB] [", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai, + freq, amp); + + double off = half_notes_away - hnai; + for (int i = -10; i <= 10; ++i) { + if (off >= (i-0.5) * 0.05 && off < (i+0.5) * 0.05) { + printf("#"); + } else { + if (i == 0) { + printf("|"); + } else { + printf("-"); + } + } + } + printf("]\n"); + +}