1 /*
2 * lingot, a musical instrument tuner.
3 *
4 * Copyright (C) 2004-2018 Iban Cereijo.
5 * Copyright (C) 2004-2008 Jairo Chapela.
6
7 *
8 * This file is part of lingot.
9 *
10 * lingot is free software; you can redistribute it and/or modify
11 * it under the terms of the GNU General Public License as published by
12 * the Free Software Foundation; either version 2 of the License, or
13 * (at your option) any later version.
14 *
15 * lingot is distributed in the hope that it will be useful,
16 * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18 * GNU General Public License for more details.
19 *
20 * You should have received a copy of the GNU General Public License
21 * along with lingot; if not, write to the Free Software
22 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
23 */
24
25 #include <stdlib.h>
26 #include <string.h>
27 #include <math.h>
28
29 #include "lingot-signal.h"
30 #include "lingot-complex.h"
31
32 /*
33 peak identification functions.
34 */
35
lingot_signal_is_peak(const FLT * signal,int index,unsigned short peak_half_width)36 static int lingot_signal_is_peak(const FLT* signal, int index,
37 unsigned short peak_half_width) {
38 register unsigned int j;
39
40 for (j = 0; j < peak_half_width; j++) {
41 if ((signal[index + j] < signal[index + j + 1])
42 || (signal[index - j] < signal[index - j - 1])) {
43 return 0;
44 }
45 }
46 return 1;
47 }
48
49 //---------------------------------------------------------------------------
50
lingot_signal_fft_bin_interpolate_quinn2_tau(FLT x)51 static FLT lingot_signal_fft_bin_interpolate_quinn2_tau(FLT x) {
52 return (0.25 * log(3 * x * x + 6 * x + 1.0)
53 - 0.102062072615966
54 * log(
55 (x + 1.0 - 0.816496580927726)
56 / (x + 1.0 + 0.816496580927726)));
57 }
58
lingot_signal_fft_bin_interpolate_quinn2(const LingotComplex y1,const LingotComplex y2,const LingotComplex y3)59 static FLT lingot_signal_fft_bin_interpolate_quinn2(const LingotComplex y1,
60 const LingotComplex y2, const LingotComplex y3) {
61 FLT absy2_2 = y2[0] * y2[0] + y2[1] * y2[1];
62 FLT ap = (y3[0] * y2[0] + y3[1] * y2[1]) / absy2_2;
63 FLT dp = -ap / (1.0 - ap);
64 FLT am = (y1[0] * y2[0] + y1[1] * y2[1]) / absy2_2;
65 FLT dm = am / (1.0 - am);
66 return 0.5 * (dp + dm)
67 + lingot_signal_fft_bin_interpolate_quinn2_tau(dp * dp)
68 - lingot_signal_fft_bin_interpolate_quinn2_tau(dm * dm);
69 }
70
71 // for qsort
lingot_signal_compare_int(const void * a,const void * b)72 static int lingot_signal_compare_int(const void *a, const void *b) {
73 return (*((int*) a) < *((int*) b)) ? -1 : 1;
74 }
75
76 // Returns a factor to multiply with in order to give more importance to higher
77 // frequency harmonics. This is to give more importance to the higher divisors
78 // for the same selected sets.
lingot_signal_frequency_penalty(FLT freq)79 static FLT lingot_signal_frequency_penalty(FLT freq) {
80 static const FLT f0 = 100;
81 static const FLT f1 = 1000;
82 static const FLT alpha0 = 0.99;
83 static const FLT alpha1 = 1;
84
85 // TODO: precompute
86 const FLT freqPenaltyA = (alpha0 - alpha1) / (f0 - f1);
87 const FLT freqPenaltyB = -(alpha0 * f1 - f0 * alpha1) / (f0 - f1);
88
89 return freq * freqPenaltyA + freqPenaltyB;
90 }
91
92 // search the fundamental peak given the SPD and its 2nd derivative
lingot_signal_estimate_fundamental_frequency(const FLT * snr,FLT freq,const LingotComplex * fft,unsigned int N,unsigned int n_peaks,unsigned int lowest_index,unsigned int highest_index,unsigned short peak_half_width,FLT delta_f_fft,FLT min_snr,FLT min_q,FLT min_freq,LingotCore * core,short * divisor)93 FLT lingot_signal_estimate_fundamental_frequency(const FLT* snr, FLT freq,
94 const LingotComplex* fft,
95 unsigned int N,
96 unsigned int n_peaks,
97 unsigned int lowest_index,
98 unsigned int highest_index,
99 unsigned short peak_half_width,
100 FLT delta_f_fft, FLT min_snr,
101 FLT min_q, FLT min_freq, LingotCore* core, short* divisor) {
102 register unsigned int i, j, m;
103 int p_index[n_peaks];
104 FLT magnitude[n_peaks];
105
106 #ifdef DRAW_MARKERS
107 core->markers_size = 0;
108 #else
109 (void)core; // Unused parameter.
110 #endif
111
112 // at this moment there are no peaks.
113 for (i = 0; i < n_peaks; i++)
114 p_index[i] = -1;
115
116 if (lowest_index < peak_half_width) {
117 lowest_index = peak_half_width;
118 }
119 if (peak_half_width + highest_index > N) {
120 highest_index = N - peak_half_width;
121 }
122
123 unsigned short n_found_peaks = 0;
124
125 // I'll get the n_peaks maximum peaks with SNR above the required.
126 for (i = lowest_index; i < highest_index; i++) {
127
128 FLT factor = 1.0;
129
130 if (freq != 0.0) {
131
132 FLT f = i * delta_f_fft;
133 if (fabs(f / freq - round(f / freq)) < 0.07) { // TODO: tune and put in conf
134 factor = 1.5; // TODO: tune and put in conf
135 }
136
137 }
138
139 FLT snri = snr[i] * factor;
140
141 if ((snri > min_snr)
142 && lingot_signal_is_peak(snr, i, peak_half_width)) {
143
144 // search a place in the maximums buffer, if it doesn't exists, the
145 // lower maximum is candidate to be replaced.
146 m = 0; // first candidate.
147 for (j = 0; j < n_peaks; j++) {
148 if (p_index[j] == -1) {
149 m = j; // there is a place.
150 break;
151 }
152
153 // if there is no place, finds the smallest peak as a candidate
154 // to be substituted.
155 if (magnitude[j] < magnitude[m]) {
156 m = j;
157 }
158 }
159
160 if (p_index[m] == -1) {
161 p_index[m] = i; // there is a place
162 magnitude[m] = snri;
163 } else if (snr[i] > snr[p_index[m]]) {
164 p_index[m] = i; // if greater
165 magnitude[m] = snri;
166 }
167
168 if (n_found_peaks < n_peaks) {
169 n_found_peaks++;
170 }
171 }
172 }
173
174 if (n_found_peaks == 0) {
175 return 0.0;
176 }
177
178 FLT maximum = 0.0;
179
180 // search the maximum peak
181 for (i = 0; i < n_found_peaks; i++)
182 if ((p_index[i] != -1) && (magnitude[i] > maximum)) {
183 maximum = magnitude[i];
184 }
185
186 // all peaks much lower than maximum are deleted.
187 int delete_counter = 0;
188 for (i = 0; i < n_found_peaks; i++) {
189 if ((p_index[i] == -1) || (magnitude[i] < maximum - 20.0)) { // TODO: conf
190 p_index[i] = N; // there are available places in the buffer.
191 delete_counter++;
192 }
193 }
194
195 qsort(p_index, n_found_peaks, sizeof(int), &lingot_signal_compare_int);
196 n_found_peaks -= delete_counter;
197
198 FLT freq_interpolated[n_found_peaks];
199 FLT delta = 0.0;
200 for (i = 0; i < n_found_peaks; i++) {
201 delta = lingot_signal_fft_bin_interpolate_quinn2(fft[p_index[i] - 1],
202 fft[p_index[i]], fft[p_index[i] + 1]);
203 freq_interpolated[i] = delta_f_fft * (p_index[i] + delta);
204
205 #ifdef DRAW_MARKERS
206 core->markers[core->markers_size++] = p_index[i];
207 #endif
208 }
209
210 // maximum ratio error
211 static const FLT ratioTol = 0.02; // TODO: tune
212 static const short max_divisor = 4;
213
214 unsigned short tone_index = 0;
215 short div = 0;
216 FLT groundFreq = 0.0;
217 FLT ratios[n_found_peaks];
218 FLT error[n_found_peaks];
219 short indices_related[n_found_peaks];
220 unsigned short n_indices_related = 0;
221
222 FLT bestQ = 0.0;
223 short best_indices_related[n_found_peaks];
224 FLT bestF = 0;
225 short bestDivisor = 1;
226
227 // possible ground frequencies
228 for (tone_index = 0; tone_index < n_found_peaks; tone_index++) {
229 for (div = 1; div <= max_divisor; div++) {
230 groundFreq = freq_interpolated[tone_index] / div;
231 if (groundFreq > min_freq) {
232 n_indices_related = 0;
233 for (i = 0; i < n_found_peaks; i++) {
234 ratios[i] = freq_interpolated[i] / groundFreq;
235 error[i] = (ratios[i] - round(ratios[i]));
236 // harmonically related frequencies
237 if (fabs(error[i]) < ratioTol) {
238 indices_related[n_indices_related++] = i;
239 }
240 }
241
242 // add the penalties for short sets and high divisors
243 // int highest_harmonic_index = n_indices_related - 1;
244 int highest_harmonic_index = 0;
245 FLT highest_harmonic_magnitude = 0.0;
246 FLT q = 0.0;
247 FLT f = 0.0;
248 // FLT snrsum = 0.0;
249 for (i = 0; i < n_indices_related; i++) {
250 // add up contributions to the quality factor
251 q += snr[p_index[indices_related[i]]]
252 * lingot_signal_frequency_penalty(groundFreq);
253 if (snr[p_index[indices_related[i]]]
254 > highest_harmonic_magnitude) {
255 highest_harmonic_index = i;
256 highest_harmonic_magnitude =
257 snr[p_index[indices_related[i]]];
258 }
259
260 // snrsum += snr[p_index[indices_related[i]]];
261 // short myDivisor = round(
262 // freq_interpolated[indices_related[i]] / groundFreq);
263 // f += snr[p_index[indices_related[i]]]
264 // * freq_interpolated[indices_related[i]] / myDivisor;
265 }
266
267 f = freq_interpolated[indices_related[highest_harmonic_index]];
268
269 // f /= snrsum;
270
271 // printf(">>>>>>>>>< f = %f\n", f);
272
273 // printf(
274 // "tone = %i, divisor = %i, gf = %f, q = %f, n = %i, f = %f, ",
275 // tone, divisor, groundFreq, q, n_indices_related, f);
276 // for (i = 0; i < n_indices_related; i++) {
277 // printf("%f ", freq_interpolated[indices_related[i]]);
278 // }
279 // printf("\n");
280
281 if (q > bestQ) {
282 bestQ = q;
283 memcpy(best_indices_related, indices_related,
284 n_indices_related * sizeof(short));
285 bestDivisor = round(f / groundFreq);
286 bestF = f;
287
288 #ifdef DRAW_MARKERS
289 core->markers_size2 = 0;
290 for (i = 0; i < n_indices_related; i++) {
291 core->markers2[core->markers_size2++] =
292 p_index[indices_related[i]];
293 }
294 #endif
295
296 // printf("%d\n", n_indices_related);
297 }
298
299 } else {
300 break;
301 }
302 }
303 }
304
305 // printf("BEST selected f = %f, f* = %f, div = %i, q = %f, n = %i, ",
306 // bestF / bestDivisor, bestDivisor, bestF, bestQ,
307 // best_n_indices_related);
308 // for (i = 0; i < best_n_indices_related; i++) {
309 // printf("%f ", freq_interpolated[best_indices_related[i]]);
310 // }
311 // printf("\n");
312
313 if ((bestF != 0.0) && (bestQ < min_q)) {
314 bestF = 0.0;
315 // printf("q < mq!!\n");
316 }
317
318 #ifdef DRAW_MARKERS
319 if (bestF == 0.0) {
320 core->markers_size2 = 0;
321 }
322 #endif
323
324 *divisor = bestDivisor;
325 return bestF;
326
327 }
328
lingot_signal_compute_noise_level(const FLT * spd,int N,int cbuffer_size,FLT * noise_level)329 void lingot_signal_compute_noise_level(const FLT* spd, int N, int cbuffer_size,
330 FLT* noise_level) {
331
332 // low pass IIR filter.
333 const FLT c = 0.1;
334 const FLT filter_a[] = { 1.0, c - 1.0 };
335 const FLT filter_b[] = { c };
336 static char initialized = 0;
337 static LingotFilter filter;
338
339 if (! initialized) {
340 initialized = 1;
341 lingot_filter_new(&filter, 1, 0, filter_a, filter_b);
342 }
343
344 lingot_filter_reset(&filter);
345
346 lingot_filter_filter(&filter, cbuffer_size, spd, noise_level);
347 lingot_filter_filter(&filter, N, spd, noise_level);
348
349 }
350
351 //---------------------------------------------------------------------------
352
353 // generates a N-samples window
lingot_signal_window(int N,FLT * out,window_type_t window_type)354 void lingot_signal_window(int N, FLT* out, window_type_t window_type) {
355 register int i;
356 switch (window_type) {
357 case HANNING:
358 for (i = 0; i < N; i++)
359 out[i] = 0.5 * (1 - cos((2.0 * M_PI * i) / (N - 1)));
360 break;
361 case HAMMING:
362 for (i = 0; i < N; i++)
363 out[i] = 0.53836 - 0.46164 * cos((2.0 * M_PI * i) / (N - 1));
364 break;
365 default:
366 break;
367 }
368 }
369