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