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

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