]> git.sesse.net Git - pitch/blob - pitch.cpp
39e20cef2b9a3a30e0fdeb600e079c2607610338
[pitch] / 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