git.sesse.net Git - pitch/blob - pitch.cpp

   1 #include <stdio.h>
   2 #include <string.h>
   3 #include <stdlib.h>
   4 #include <unistd.h>
   5 #include <fcntl.h>
   6 #include <complex>
   7 #include <cassert>
   8 #include <algorithm>
   9 #include <fftw3.h>
  10 #include <sys/ioctl.h>
  11 #include <linux/soundcard.h>
  12
  13 #include "pitchdetector.h"
  14
  15 #define BASE_PITCH      440.0
  16 #define SAMPLE_RATE     22050
  17 #define FFT_LENGTH      4096     /* in samples */
  18 #define PAD_FACTOR      2        /* 1/pf of the FFT samples are real samples, the rest are padding */
  19 #define OVERLAP         4        /* 1/ol samples will be replaced in the buffer every frame. Should be
  20                                   * a multiple of 2 for the Hamming window (see
  21                                   * http://www-ccrma.stanford.edu/~jos/parshl/Choice_Hop_Size.html).
  22                                   */
  23
  24 #define EQUAL_TEMPERAMENT     0
  25 #define WELL_TEMPERED_GUITAR  1
  26
  27 #define TUNING WELL_TEMPERED_GUITAR
  28
  29 int get_dsp_fd();
  30 void read_chunk(int fd, short *in, unsigned num_samples);
  31 void print_spectrogram(double freq, double amp);
  32 void write_sine(int dsp_fd, double freq, unsigned num_samples);
  33
  34 int main()
  35 {
  36         PitchDetector pd(SAMPLE_RATE, FFT_LENGTH, PAD_FACTOR, OVERLAP);
  37
  38         int fd = get_dsp_fd();
  39         for ( ;; ) {
  40                 short buf[FFT_LENGTH / PAD_FACTOR / OVERLAP];
  41
  42                 read_chunk(fd, buf, FFT_LENGTH / PAD_FACTOR / OVERLAP);
  43                 std::pair<double, double> peak = pd.detect_pitch(buf);
  44
  45                 if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) {
  46 #if TUNING == WELL_TEMPERED_GUITAR
  47                         printf("......\n");
  48 #else
  49                         printf("............\n");
  50 #endif
  51                 } else {
  52                         print_spectrogram(peak.first, peak.second - log10(FFT_LENGTH));
  53                 }
  54         }
  55 }
  56
  57 int get_dsp_fd()
  58 {
  59         int fd = open("/dev/dsp", O_RDWR);
  60         if (fd == -1) {
  61                 perror("/dev/dsp");
  62                 exit(1);
  63         }
  64
  65         ioctl(3, SNDCTL_DSP_RESET, 0);
  66
  67         int fmt = AFMT_S16_LE;   // FIXME
  68         ioctl(fd, SNDCTL_DSP_SETFMT, &fmt);
  69
  70         int chan = 1;
  71         ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan);
  72
  73         int rate = SAMPLE_RATE;
  74         ioctl(fd, SOUND_PCM_WRITE_RATE, &rate);
  75
  76         int max_fragments = 2;
  77         int frag_shift = ffs(FFT_LENGTH / OVERLAP) - 1;
  78         int fragments = (max_fragments << 16) | frag_shift;
  79         ioctl(fd, SNDCTL_DSP_SETFRAGMENT, &fragments);
  80
  81         ioctl(3, SNDCTL_DSP_SYNC, 0);
  82
  83         return fd;
  84 }
  85
  86 #if 1
  87 void read_chunk(int fd, short *in, unsigned num_samples)
  88 {
  89         int ret;
  90
  91         ret = read(fd, in, num_samples * sizeof(short));
  92         if (ret == 0) {
  93                 printf("EOF\n");
  94                 exit(0);
  95         }
  96
  97         if (ret != int(num_samples * sizeof(short))) {
  98                 // blah
  99                 perror("read");
 100                 exit(1);
 101         }
 102 }
 103 #else
 104 // make a pure 440hz sine for testing
 105 void read_chunk(int fd, short *in, unsigned num_samples)
 106 {
 107         static double theta = 0.0;
 108         for (unsigned i = 0; i < num_samples; ++i) {
 109                 in[i] = 32768.0 * cos(theta);
 110                 theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
 111         }
 112 }
 113 #endif
 114
 115 void write_sine(int dsp_fd, double freq, unsigned num_samples)
 116 {
 117         static double theta = 0.0;
 118         short buf[num_samples];
 119
 120         for (unsigned i = 0; i < num_samples; ++i) {
 121                 buf[i] = short(cos(theta) * 16384.0);
 122                 theta += 2.0 * M_PI * freq / double(SAMPLE_RATE);
 123         }
 124
 125         write(dsp_fd, buf, num_samples * sizeof(short));
 126 }
 127
 128 std::string freq_to_tonename(double freq)
 129 {
 130         std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
 131         double half_notes_away = 12.0 * log2(freq / BASE_PITCH) - 3.0;
 132         int hnai = int(floor(half_notes_away + 0.5));
 133         int octave = (hnai + 48) / 12;
 134
 135         char buf[256];
 136         sprintf(buf, "%s%d + %.2f [%d]", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai, hnai);
 137         return buf;
 138 }
 139
 140 #if TUNING == EQUAL_TEMPERAMENT
 141 void print_spectrogram(double freq, double amp)
 142 {
 143         std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
 144         double half_notes_away = 12.0 * log2(freq / BASE_PITCH) - 3.0;
 145         int hnai = int(floor(half_notes_away + 0.5));
 146         int octave = (hnai + 48) / 12;
 147
 148         for (int i = 0; i < 12; ++i)
 149                 if (i == ((hnai % 12) + 12) % 12)
 150                         printf("#");
 151                 else
 152                         printf(".");
 153
 154         printf(" (%-2s%d %+.2f, %5.2fHz) [%5.2fdB]  [", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai,
 155                 freq, amp);
 156
 157         double off = half_notes_away - hnai;
 158         for (int i = -10; i <= 10; ++i) {
 159                 if (off >= (i-0.5) * 0.05 && off < (i+0.5) * 0.05) {
 160                         printf("#");
 161                 } else {
 162                         if (i == 0) {
 163                                 printf("|");
 164                         } else {
 165                                 printf("-");
 166                         }
 167                 }
 168         }
 169         printf("]\n");
 170
 171 }
 172 #else
 173 struct note {
 174         char notename[16];
 175         double freq;
 176 };
 177 static note notes[] = {
 178         { "E-3", BASE_PITCH/4.0 * (3.0/4.0) },
 179         { "A-3", BASE_PITCH/4.0 },
 180         { "D-4", BASE_PITCH/4.0 * (4.0/3.0) },
 181         { "G-4", BASE_PITCH/4.0 * (4.0/3.0)*(4.0/3.0) },
 182         { "B-4", BASE_PITCH * (3.0/4.0)*(3.0/4.0) },
 183         { "E-5", BASE_PITCH * (3.0/4.0) }
 184 };
 185
 186 void print_spectrogram(double freq, double amp)
 187 {
 188         double best_away = 999999999.9;
 189         unsigned best_away_ind = 0;
 190
 191         for (unsigned i = 0; i < sizeof(notes)/sizeof(note); ++i) {
 192                 double half_notes_away = 12.0 * log2(freq / notes[i].freq);
 193                 if (fabs(half_notes_away) < fabs(best_away)) {
 194                         best_away = half_notes_away;
 195                         best_away_ind = i;
 196                 }
 197         }
 198
 199         for (unsigned i = 0; i < sizeof(notes)/sizeof(note); ++i)
 200                 if (i == best_away_ind)
 201                         printf("#");
 202                 else
 203                         printf(".");
 204
 205         printf(" (%s %+.2f, %5.2fHz) [%5.2fdB]  [", notes[best_away_ind].notename, best_away, freq, amp);
 206
 207         // coarse tuning
 208         for (int i = -10; i <= 10; ++i) {
 209                 if (best_away >= (i-0.5) * 0.05 && best_away < (i+0.5) * 0.05) {
 210                         printf("#");
 211                 } else {
 212                         if (i == 0) {
 213                                 printf("|");
 214                         } else {
 215                                 printf("-");
 216                         }
 217                 }
 218         }
 219         printf("]  [");
 220
 221         // fine tuning
 222         for (int i = -10; i <= 10; ++i) {
 223                 if (best_away >= (i-0.5) * 0.01 && best_away < (i+0.5) * 0.01) {
 224                         printf("#");
 225                 } else {
 226                         if (i == 0) {
 227                                 printf("|");
 228                         } else {
 229                                 printf("-");
 230                         }
 231                 }
 232         }
 233         printf("]\n");
 234 }
 235 #endif