Guitarix
gx_pitch_tracker.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2009, 2010 Hermann Meyer, James Warden, Andreas Degert
3  * Copyright (C) 2011 Pete Shorthose
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
18  * --------------------------------------------------------------------------
19  */
20 
21 /* ------- This is the guitarix tuner, part of gx_engine_audio.cpp ------- */
22 
23 #include "engine.h"
24 
25 /****************************************************************
26  ** Pitch Tracker
27  **
28  ** some code and ideas taken from K4Guitune (William Spinelli)
29  ** changed to NSDF method (some code from tartini / Philip McLeod)
30  **
31  */
32 
33 namespace gx_engine {
34 
35 // downsampling factor
36 static const int DOWNSAMPLE = 2;
37 static const float SIGNAL_THRESHOLD_ON = 0.001;
38 static const float SIGNAL_THRESHOLD_OFF = 0.0009;
39 static const float TRACKER_PERIOD = 0.1;
40 // The size of the read buffer
41 static const int FFT_SIZE = 2048;
42 
43 
44 void *PitchTracker::static_run(void *p) {
45  (reinterpret_cast<PitchTracker *>(p))->run();
46  return NULL;
47 }
48 
50  : error(false),
51  busy(false),
52  tick(0),
53  m_pthr(0),
54  resamp(),
55  m_sampleRate(),
56  fixed_sampleRate(41000),
57  m_freq(-1),
58  signal_threshold_on(SIGNAL_THRESHOLD_ON),
59  signal_threshold_off(SIGNAL_THRESHOLD_OFF),
60  tracker_period(TRACKER_PERIOD),
61  m_buffersize(),
62  m_fftSize(),
63  m_buffer(new float[FFT_SIZE]),
64  m_bufferIndex(0),
65  m_input(new float[FFT_SIZE]),
66  m_audioLevel(false),
67  m_fftwPlanFFT(0),
68  m_fftwPlanIFFT(0) {
69  const int size = FFT_SIZE + (FFT_SIZE+1) / 2;
70  m_fftwBufferTime = reinterpret_cast<float*>
71  (fftwf_malloc(size * sizeof(*m_fftwBufferTime)));
72  m_fftwBufferFreq = reinterpret_cast<float*>
73  (fftwf_malloc(size * sizeof(*m_fftwBufferFreq)));
74 
75  memset(m_buffer, 0, FFT_SIZE * sizeof(*m_buffer));
76  memset(m_input, 0, FFT_SIZE * sizeof(*m_input));
77  memset(m_fftwBufferTime, 0, size * sizeof(*m_fftwBufferTime));
78  memset(m_fftwBufferFreq, 0, size * sizeof(*m_fftwBufferFreq));
79 
80  sem_init(&m_trig, 0, 0);
81 
82  if (!m_buffer || !m_input || !m_fftwBufferTime || !m_fftwBufferFreq) {
83  gx_print_error("PitchTracker", "out of memory");
84  error = true;
85  }
86 }
87 
88 
90  stop_thread();
91  fftwf_destroy_plan(m_fftwPlanFFT);
92  fftwf_destroy_plan(m_fftwPlanIFFT);
93  fftwf_free(m_fftwBufferTime);
94  fftwf_free(m_fftwBufferFreq);
95  delete[] m_input;
96  delete[] m_buffer;
97 }
98 
100  if (v) {
101  signal_threshold_on = SIGNAL_THRESHOLD_ON * 5;
102  signal_threshold_off = SIGNAL_THRESHOLD_OFF * 5;
103  tracker_period = TRACKER_PERIOD / 10;
104  } else {
105  signal_threshold_on = SIGNAL_THRESHOLD_ON;
106  signal_threshold_off = SIGNAL_THRESHOLD_OFF;
107  tracker_period = TRACKER_PERIOD;
108  }
109 }
110 
111 bool PitchTracker::setParameters(int priority, int policy, int sampleRate, int buffersize) {
112  assert(buffersize <= FFT_SIZE);
113 
114  if (error) {
115  return false;
116  }
117 
118  m_sampleRate = fixed_sampleRate / DOWNSAMPLE;
119  resamp.setup(sampleRate, m_sampleRate, 1, 16); // 16 == least quality
120 
121  if (m_buffersize != buffersize) {
122  m_buffersize = buffersize;
123  m_fftSize = m_buffersize + (m_buffersize+1) / 2;
124  fftwf_destroy_plan(m_fftwPlanFFT);
125  fftwf_destroy_plan(m_fftwPlanIFFT);
126  m_fftwPlanFFT = fftwf_plan_r2r_1d(
127  m_fftSize, m_fftwBufferTime, m_fftwBufferFreq,
128  FFTW_R2HC, FFTW_ESTIMATE);
129  m_fftwPlanIFFT = fftwf_plan_r2r_1d(
130  m_fftSize, m_fftwBufferFreq, m_fftwBufferTime,
131  FFTW_HC2R, FFTW_ESTIMATE);
132  }
133 
134  if (!m_fftwPlanFFT || !m_fftwPlanIFFT) {
135  error = true;
136  gx_print_error("PitchTracker", "can't allocate FFTW plan");
137  return false;
138  }
139 
140  if (!m_pthr) {
141  start_thread(priority, policy);
142  }
143  return !error;
144 }
145 
147  if (m_pthr) {
148  pthread_cancel (m_pthr);
149  pthread_join (m_pthr, NULL);
150  m_pthr = 0;
151  }
152 }
153 
154 void PitchTracker::start_thread(int priority, int policy) {
155  pthread_attr_t attr;
156  struct sched_param spar;
157  spar.sched_priority = priority;
158  pthread_attr_init(&attr);
159  pthread_attr_setdetachstate(&attr,PTHREAD_CREATE_JOINABLE );
160  pthread_setcancelstate (PTHREAD_CANCEL_ENABLE, NULL);
161  pthread_attr_setschedpolicy(&attr, policy);
162  pthread_attr_setschedparam(&attr, &spar);
163  pthread_attr_setscope(&attr, PTHREAD_SCOPE_SYSTEM);
164  pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
165  // pthread_attr_setstacksize(&attr, 0x10000);
166  if (pthread_create(&m_pthr, &attr, static_run,
167  reinterpret_cast<void*>(this))) {
168  error = true;
169  if (errno == EPERM) {
171  "PitchTracker",
172  _("no permission to create realtime thread - please check your system configuration - tuner not started"));
173  } else {
175  "PitchTracker",
176  _("error creating realtime thread - tuner not started"));
177  }
178  }
179  pthread_attr_destroy(&attr);
180 }
181 
182 void PitchTracker::init(int priority, int policy, unsigned int samplerate) {
183  setParameters(priority, policy, samplerate, FFT_SIZE);
184 }
185 
187  tick = 0;
188  m_bufferIndex = 0;
189  resamp.reset();
190  m_freq = -1;
191 }
192 
193 void PitchTracker::add(int count, float* input) {
194  if (error) {
195  return;
196  }
197  resamp.inp_count = count;
198  resamp.inp_data = input;
199  for (;;) {
200  resamp.out_data = &m_buffer[m_bufferIndex];
201  int n = FFT_SIZE - m_bufferIndex;
202  resamp.out_count = n;
203  resamp.process();
204  n -= resamp.out_count; // n := number of output samples
205  if (!n) { // all soaked up by filter
206  return;
207  }
208  m_bufferIndex = (m_bufferIndex + n) % FFT_SIZE;
209  if (resamp.inp_count == 0) {
210  break;
211  }
212  }
213  if (++tick * count >= m_sampleRate * DOWNSAMPLE * tracker_period) {
214  if (busy) {
215  return;
216  }
217  busy = true;
218  tick = 0;
219  copy();
220  sem_post(&m_trig);
221  }
222 }
223 
224 void PitchTracker::copy() {
225  int start = (FFT_SIZE + m_bufferIndex - m_buffersize) % FFT_SIZE;
226  int end = (FFT_SIZE + m_bufferIndex) % FFT_SIZE;
227  int cnt = 0;
228  if (start >= end) {
229  cnt = FFT_SIZE - start;
230  memcpy(m_input, &m_buffer[start], cnt * sizeof(*m_input));
231  start = 0;
232  }
233  memcpy(&m_input[cnt], &m_buffer[start], (end - start) * sizeof(*m_input));
234 }
235 
236 inline float sq(float x) {
237  return x * x;
238 }
239 
240 inline void parabolaTurningPoint(float y_1, float y0, float y1, float xOffset, float *x) {
241  float yTop = y_1 - y1;
242  float yBottom = y1 + y_1 - 2 * y0;
243  if (yBottom != 0.0) {
244  *x = xOffset + yTop / (2 * yBottom);
245  } else {
246  *x = xOffset;
247  }
248 }
249 
250 static int findMaxima(float *input, int len, int *maxPositions, int *length, int maxLen) {
251  int pos = 0;
252  int curMaxPos = 0;
253  int overallMaxIndex = 0;
254 
255  while (pos < (len-1)/3 && input[pos] > 0.0) {
256  pos += 1; // find the first negitive zero crossing
257  }
258  while (pos < len-1 && input[pos] <= 0.0) {
259  pos += 1; // loop over all the values below zero
260  }
261  if (pos == 0) {
262  pos = 1; // can happen if output[0] is NAN
263  }
264  while (pos < len-1) {
265  if (input[pos] > input[pos-1] && input[pos] >= input[pos+1]) { // a local maxima
266  if (curMaxPos == 0) {
267  curMaxPos = pos; // the first maxima (between zero crossings)
268  } else if (input[pos] > input[curMaxPos]) {
269  curMaxPos = pos; // a higher maxima (between the zero crossings)
270  }
271  }
272  pos += 1;
273  if (pos < len-1 && input[pos] <= 0.0) { // a negative zero crossing
274  if (curMaxPos > 0) { // if there was a maximum
275  maxPositions[*length] = curMaxPos; // add it to the vector of maxima
276  *length += 1;
277  if (overallMaxIndex == 0) {
278  overallMaxIndex = curMaxPos;
279  } else if (input[curMaxPos] > input[overallMaxIndex]) {
280  overallMaxIndex = curMaxPos;
281  }
282  if (*length >= maxLen) {
283  return overallMaxIndex;
284  }
285  curMaxPos = 0; // clear the maximum position, so we start looking for a new ones
286  }
287  while (pos < len-1 && input[pos] <= 0.0) {
288  pos += 1; // loop over all the values below zero
289  }
290  }
291  }
292  if (curMaxPos > 0) { // if there was a maximum in the last part
293  maxPositions[*length] = curMaxPos; // add it to the vector of maxima
294  *length += 1;
295  if (overallMaxIndex == 0) {
296  overallMaxIndex = curMaxPos;
297  } else if (input[curMaxPos] > input[overallMaxIndex]) {
298  overallMaxIndex = curMaxPos;
299  }
300  curMaxPos = 0; // clear the maximum position, so we start looking for a new ones
301  }
302  return overallMaxIndex;
303 }
304 
305 static int findsubMaximum(float *input, int len, float threshold) {
306  int indices[10];
307  int length = 0;
308  int overallMaxIndex = findMaxima(input, len, indices, &length, 10);
309  if (length == 0) {
310  return -1;
311  }
312  threshold += (1.0 - threshold) * (1.0 - input[overallMaxIndex]);
313  float cutoff = input[overallMaxIndex] * threshold;
314  for (int j = 0; j < length; j++) {
315  if (input[indices[j]] >= cutoff) {
316  return indices[j];
317  }
318  }
319  // should never get here
320  return -1;
321 }
322 
323 void PitchTracker::run() {
324  for (;;) {
325  busy = false;
326  sem_wait(&m_trig);
327  if (error) {
328  continue;
329  }
330  float sum = 0.0;
331  for (int k = 0; k < m_buffersize; ++k) {
332  sum += fabs(m_input[k]);
333  }
334  float threshold = (m_audioLevel ? signal_threshold_off : signal_threshold_on);
335  m_audioLevel = (sum / m_buffersize >= threshold);
336  if ( m_audioLevel == false ) {
337  if (m_freq != 0) {
338  m_freq = 0;
339  new_freq();
340  }
341  continue;
342  }
343 
344  memcpy(m_fftwBufferTime, m_input, m_buffersize * sizeof(*m_fftwBufferTime));
345  memset(m_fftwBufferTime+m_buffersize, 0, (m_fftSize - m_buffersize) * sizeof(*m_fftwBufferTime));
346  fftwf_execute(m_fftwPlanFFT);
347  for (int k = 1; k < m_fftSize/2; k++) {
348  m_fftwBufferFreq[k] = sq(m_fftwBufferFreq[k]) + sq(m_fftwBufferFreq[m_fftSize-k]);
349  m_fftwBufferFreq[m_fftSize-k] = 0.0;
350  }
351  m_fftwBufferFreq[0] = sq(m_fftwBufferFreq[0]);
352  m_fftwBufferFreq[m_fftSize/2] = sq(m_fftwBufferFreq[m_fftSize/2]);
353 
354  fftwf_execute(m_fftwPlanIFFT);
355 
356  double sumSq = 2.0 * static_cast<double>(m_fftwBufferTime[0]) / static_cast<double>(m_fftSize);
357  for (int k = 0; k < m_fftSize - m_buffersize; k++) {
358  m_fftwBufferTime[k] = m_fftwBufferTime[k+1] / static_cast<float>(m_fftSize);
359  }
360 
361  int count = (m_buffersize + 1) / 2;
362  for (int k = 0; k < count; k++) {
363  sumSq -= sq(m_input[m_buffersize-1-k]) + sq(m_input[k]);
364  // dividing by zero is very slow, so deal with it seperately
365  if (sumSq > 0.0) {
366  m_fftwBufferTime[k] *= 2.0 / sumSq;
367  } else {
368  m_fftwBufferTime[k] = 0.0;
369  }
370  }
371  const float thres = 0.99; // was 0.6
372  int maxAutocorrIndex = findsubMaximum(m_fftwBufferTime, count, thres);
373 
374  float x = 0.0;
375  if (maxAutocorrIndex >= 0) {
376  parabolaTurningPoint(m_fftwBufferTime[maxAutocorrIndex-1],
377  m_fftwBufferTime[maxAutocorrIndex],
378  m_fftwBufferTime[maxAutocorrIndex+1],
379  maxAutocorrIndex+1, &x);
380  x = m_sampleRate / x;
381  if (x > 999.0) { // precision drops above 1000 Hz
382  x = 0.0;
383  }
384  }
385  if (m_freq != x) {
386  m_freq = x;
387  new_freq();
388  }
389  }
390 }
391 
393  return m_freq <= 0.0 ? 1000.0 : 12 * log2f(2.272727e-03f * m_freq);
394 }
395 
396 }
CmdConnection::msg_type end
Definition: jsonrpc.cpp:258
CmdConnection::msg_type start
Definition: jsonrpc.cpp:257
void parabolaTurningPoint(float y_1, float y0, float y1, float xOffset, float *x)
void set_fast_note_detection(bool v)
void gx_print_error(const char *, const std::string &)
Definition: gx_logging.cpp:166
void init(int priority, int policy, unsigned int samplerate)
float sq(float x)
void add(int count, float *input)
Glib::Dispatcher new_freq