Make a fine tuning display for the guitar; useful to get it "just right".
[pitch] / 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