Fix missing #include of string.h.
[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 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