10 #include <linux/soundcard.h>
12 #define SAMPLE_RATE 22050
13 #define FFT_LENGTH 4096 /* in samples */
14 #define PAD_FACTOR 2 /* 1/pf of the FFT samples are real samples, the rest are padding */
15 #define OVERLAP 4 /* 1/ol samples will be replaced in the buffer every frame. Should be
16 * a multiple of 2 for the Hamming window (see
17 * http://www-ccrma.stanford.edu/~jos/parshl/Choice_Hop_Size.html).
20 #define EQUAL_TEMPERAMENT 0
21 #define WELL_TEMPERED_GUITAR 1
23 #define TUNING WELL_TEMPERED_GUITAR
26 void read_chunk(int fd, double *in, unsigned num_samples);
27 void apply_window(double *in, double *out, unsigned num_samples);
28 std::pair<double, double> find_peak(double *in, unsigned num_samples);
29 void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples);
30 std::pair<double, double> adjust_for_overtones(std::pair<double, double> base, double *in, unsigned num_samples);
31 double bin_to_freq(double bin, unsigned num_samples);
32 double freq_to_bin(double freq, unsigned num_samples);
33 std::string freq_to_tonename(double freq);
34 std::pair<double, double> interpolate_peak(double ym1, double y0, double y1);
35 void print_spectrogram(double freq, double amp);
36 void write_sine(int dsp_fd, double freq, unsigned num_samples);
40 double *in, *in_windowed;
41 std::complex<double> *out;
46 in = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH / PAD_FACTOR));
47 in_windowed = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH));
48 out = reinterpret_cast<std::complex<double> *> (fftw_malloc(sizeof(std::complex<double>) * (FFT_LENGTH / 2 + 1)));
49 bins = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * (FFT_LENGTH / 2 + 1)));
51 memset(in, 0, sizeof(double) * FFT_LENGTH / PAD_FACTOR);
54 p = fftw_plan_dft_r2c_1d(FFT_LENGTH, in_windowed, reinterpret_cast<fftw_complex *> (out), FFTW_ESTIMATE);
56 int fd = get_dsp_fd();
58 read_chunk(fd, in, FFT_LENGTH);
59 apply_window(in, in_windowed, FFT_LENGTH);
61 find_peak_magnitudes(out, bins, FFT_LENGTH);
62 std::pair<double, double> peak = find_peak(bins, FFT_LENGTH);
63 peak = adjust_for_overtones(peak, bins, FFT_LENGTH);
65 if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) {
66 #if TUNING == WELL_TEMPERED_GUITAR
69 printf("............\n");
72 print_spectrogram(peak.first, peak.second - log10(FFT_LENGTH));
79 int fd = open("/dev/dsp", O_RDWR);
85 ioctl(3, SNDCTL_DSP_RESET, 0);
87 int fmt = AFMT_S16_LE; // FIXME
88 ioctl(fd, SNDCTL_DSP_SETFMT, &fmt);
91 ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan);
93 int rate = SAMPLE_RATE;
94 ioctl(fd, SOUND_PCM_WRITE_RATE, &rate);
96 int max_fragments = 2;
97 int frag_shift = ffs(FFT_LENGTH / OVERLAP) - 1;
98 int fragments = (max_fragments << 16) | frag_shift;
99 ioctl(fd, SNDCTL_DSP_SETFRAGMENT, &fragments);
101 ioctl(3, SNDCTL_DSP_SYNC, 0);
107 void read_chunk(int fd, double *in, unsigned num_samples)
110 unsigned to_read = num_samples / PAD_FACTOR / OVERLAP;
113 memmove(in, in + to_read, (num_samples / PAD_FACTOR - to_read) * sizeof(double));
115 ret = read(fd, buf, to_read * sizeof(short));
121 if (ret != int(to_read * sizeof(short))) {
127 for (unsigned i = 0; i < to_read; ++i)
128 in[i + (num_samples / PAD_FACTOR - to_read)] = double(buf[i]);
131 // make a pure 440hz sine for testing
132 void read_chunk(int fd, double *in, unsigned num_samples)
134 static double theta = 0.0;
135 for (unsigned i = 0; i < num_samples; ++i) {
137 theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
142 void write_sine(int dsp_fd, double freq, unsigned num_samples)
144 static double theta = 0.0;
145 short buf[num_samples];
147 for (unsigned i = 0; i < num_samples; ++i) {
148 buf[i] = short(cos(theta) * 16384.0);
149 theta += 2.0 * M_PI * freq / double(SAMPLE_RATE);
152 write(dsp_fd, buf, num_samples * sizeof(short));
155 // Apply a standard Hamming window to our input data.
156 void apply_window(double *in, double *out, unsigned num_samples)
158 static double *win_data;
159 static unsigned win_len;
160 static bool win_inited = false;
162 // Initialize the window for the first time
164 win_len = num_samples / PAD_FACTOR;
165 win_data = new double[win_len];
167 for (unsigned i = 0; i < win_len; ++i) {
168 win_data[i] = 0.54 - 0.46 * cos(2.0 * M_PI * double(i) / double(win_len - 1));
174 assert(win_len == num_samples / PAD_FACTOR);
176 for (unsigned i = 0; i < win_len; ++i) {
177 out[i] = in[i] * win_data[i];
179 for (unsigned i = win_len; i < num_samples; ++i) {
184 void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples)
186 for (unsigned i = 0; i < num_samples / 2 + 1; ++i)
190 std::pair<double, double> find_peak(double *in, unsigned num_samples)
192 double best_peak = in[0];
193 unsigned best_bin = 0;
195 for (unsigned i = 1; i < num_samples / 2 + 1; ++i) {
196 if (in[i] > best_peak) {
202 if (best_bin == 0 || best_bin == num_samples / 2) {
203 return std::make_pair(-1.0, 0.0);
207 printf("undertone strength: %+4.2f %+4.2f %+4.2f [%+4.2f] %+4.2f %+4.2f %+4.2f\n",
208 20.0 * log10(in[best_bin*4] / FFT_LENGTH),
209 20.0 * log10(in[best_bin*3] / FFT_LENGTH),
210 20.0 * log10(in[best_bin*2] / FFT_LENGTH),
211 20.0 * log10(in[best_bin] / FFT_LENGTH),
212 20.0 * log10(in[best_bin/2] / FFT_LENGTH),
213 20.0 * log10(in[best_bin/3] / FFT_LENGTH),
214 20.0 * log10(in[best_bin/4] / FFT_LENGTH));
217 // see if we might have hit an overtone (set a limit of 5dB)
218 for (unsigned i = 4; i >= 1; --i) {
219 if (best_bin != best_bin / i &&
220 20.0 * log10(in[best_bin] / in[best_bin / i]) < 5.0f) {
222 printf("Overtone of degree %u!\n", i);
226 // consider sliding one bin up or down
227 if (best_bin > 0 && in[best_bin - 1] > in[best_bin] && in[best_bin - 1] > in[best_bin - 2]) {
229 } else if (best_bin < num_samples / 2 && in[best_bin + 1] > in[best_bin] && in[best_bin + 1] > in[best_bin + 2]) {
237 if (best_bin == 0 || best_bin == num_samples / 2) {
238 return std::make_pair(-1.0, 0.0);
240 std::pair<double, double> peak =
241 interpolate_peak(in[best_bin - 1],
245 return std::make_pair(bin_to_freq(double(best_bin) + peak.first, num_samples), peak.second);
248 // it's perhaps not ideal to _first_ find the peak and _then_ the harmonics --
249 // ideally, one should find the set of all peaks and then determine the likely
250 // base from that... something for later. :-)
251 std::pair<double, double> adjust_for_overtones(std::pair<double, double> base, double *in, unsigned num_samples)
253 double mu = base.first, var = 1.0 / (base.second * base.second);
255 //printf("Base at %.2f (amp=%5.2fdB)\n", base.first, base.second);
257 for (unsigned i = 2; i < 10; ++i) {
258 unsigned middle = unsigned(floor(freq_to_bin(base.first, num_samples) * i + 0.5));
259 unsigned lower = middle - (i+1)/2, upper = middle + (i+1)/2;
261 if (upper >= num_samples)
262 upper = num_samples - 2;
264 // printf("Searching in [%u,%u] = %f..%f\n", lower, upper, bin_to_freq(lower, num_samples), bin_to_freq(upper, num_samples));
266 // search for a peak in this interval
267 double best_harmonics_freq = -1.0;
268 double best_harmonics_amp = -1.0;
269 for (unsigned j = lower; j <= upper; ++j) {
270 if (in[j] > in[j-1] && in[j] > in[j+1]) {
271 std::pair<double, double> peak =
272 interpolate_peak(in[j - 1],
276 if (peak.second > best_harmonics_amp) {
277 best_harmonics_freq = bin_to_freq(j + peak.first, num_samples);
278 best_harmonics_amp = peak.second;
283 if (best_harmonics_amp <= 0.0)
286 //printf("Found overtone %u at %.2f (amp=%5.2fdB)\n", i, best_harmonics_freq,
287 // best_harmonics_amp);
289 double this_mu = best_harmonics_freq / double(i);
290 double this_var = 1.0 / (best_harmonics_amp * best_harmonics_amp);
292 double k = var / (var + this_var);
293 mu = (1.0 - k) * mu + k * this_mu;
296 return std::make_pair(mu, base.second);
299 double bin_to_freq(double bin, unsigned num_samples)
301 return bin * SAMPLE_RATE / double(num_samples);
303 double freq_to_bin(double freq, unsigned num_samples)
305 return freq * double(num_samples) / double(SAMPLE_RATE);
309 * Given three bins, find the interpolated real peak based
310 * on their magnitudes. To do this, we execute the following
313 * Fit a polynomial of the form ax^2 + bx + c = 0 to the data
314 * we have. Maple readily yields our coefficients, assuming
315 * that we have the values at x=-1, x=0 and x=1:
317 * > f := x -> a*x^2 + b*x + c;
320 * f := x -> a x + b x + c
322 * > cf := solve({ f(-1) = ym1, f(0) = y0, f(1) = y1 }, { a, b, c });
325 * cf := {c = y0, b = ---- - ---, a = ---- + --- - y0}
328 * Now let's find the maximum point for the polynomial (it has to be
329 * a maximum, since y0 is the greatest value):
331 * > xmax := solve(subs(cf, diff(f(x), x)) = 0, x);
334 * xmax := -------------------
335 * 2 (y1 + ym1 - 2 y0)
337 * We could go further and insert {fmax,a,b,c} into the original
338 * polynomial, but it just gets hairy. We calculate a, b and c separately
341 * http://www-ccrma.stanford.edu/~jos/parshl/Peak_Detection_Steps_3.html
342 * claims that detection is almost twice as good when using dB scale instead
343 * of linear scale for the input values, so we use that. (As a tiny bonus,
344 * we get back dB scale from the function.)
346 std::pair<double, double> interpolate_peak(double ym1, double y0, double y1)
357 double a = 0.5 * y1 + 0.5 * ym1 - y0;
358 double b = 0.5 * y1 - 0.5 * ym1;
361 double xmax = (ym1 - y1) / (2.0 * (y1 + ym1 - 2.0 * y0));
362 double ymax = 20.0 * (a * xmax * xmax + b * xmax + c) - 90.0;
364 return std::make_pair(xmax, ymax);
367 std::string freq_to_tonename(double freq)
369 std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
370 double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0;
371 int hnai = int(floor(half_notes_away + 0.5));
372 int octave = (hnai + 48) / 12;
375 sprintf(buf, "%s%d + %.2f [%d]", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai, hnai);
379 #if TUNING == EQUAL_TEMPERAMENT
380 void print_spectrogram(double freq, double amp)
382 std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
383 double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0;
384 int hnai = int(floor(half_notes_away + 0.5));
385 int octave = (hnai + 48) / 12;
387 for (int i = 0; i < 12; ++i)
388 if (i == ((hnai % 12) + 12) % 12)
393 printf(" (%-2s%d %+.2f, %5.2fHz) [%5.2fdB] [", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai,
396 double off = half_notes_away - hnai;
397 for (int i = -10; i <= 10; ++i) {
398 if (off >= (i-0.5) * 0.05 && off < (i+0.5) * 0.05) {
416 static note notes[] = {
417 { "E-3", 110.0 * (3.0/4.0) },
419 { "D-4", 110.0 * (4.0/3.0) },
420 { "G-4", 110.0 * (4.0/3.0)*(4.0/3.0) },
421 { "B-4", 440.0 * (3.0/4.0)*(3.0/4.0) },
422 { "E-5", 440.0 * (3.0/4.0) }
425 void print_spectrogram(double freq, double amp)
427 double best_away = 999999999.9;
428 unsigned best_away_ind = 0;
430 for (unsigned i = 0; i < sizeof(notes)/sizeof(note); ++i) {
431 double half_notes_away = 12.0 * log2(freq / notes[i].freq);
432 if (fabs(half_notes_away) < fabs(best_away)) {
433 best_away = half_notes_away;
438 for (unsigned i = 0; i < sizeof(notes)/sizeof(note); ++i)
439 if (i == best_away_ind)
444 printf(" (%s %+.2f, %5.2fHz) [%5.2fdB] [", notes[best_away_ind].notename, best_away, freq, amp);
446 for (int i = -10; i <= 10; ++i) {
447 if (best_away >= (i-0.5) * 0.05 && best_away < (i+0.5) * 0.05) {