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;
41 static const int FFT_SIZE = 2048;
44 void *PitchTracker::static_run(
void *p) {
45 (reinterpret_cast<PitchTracker *>(p))->run();
56 fixed_sampleRate(41000),
58 signal_threshold_on(SIGNAL_THRESHOLD_ON),
59 signal_threshold_off(SIGNAL_THRESHOLD_OFF),
60 tracker_period(TRACKER_PERIOD),
63 m_buffer(new float[FFT_SIZE]),
65 m_input(new float[FFT_SIZE]),
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)));
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));
80 sem_init(&m_trig, 0, 0);
82 if (!m_buffer || !m_input || !m_fftwBufferTime || !m_fftwBufferFreq) {
91 fftwf_destroy_plan(m_fftwPlanFFT);
92 fftwf_destroy_plan(m_fftwPlanIFFT);
93 fftwf_free(m_fftwBufferTime);
94 fftwf_free(m_fftwBufferFreq);
101 signal_threshold_on = SIGNAL_THRESHOLD_ON * 5;
102 signal_threshold_off = SIGNAL_THRESHOLD_OFF * 5;
103 tracker_period = TRACKER_PERIOD / 10;
105 signal_threshold_on = SIGNAL_THRESHOLD_ON;
106 signal_threshold_off = SIGNAL_THRESHOLD_OFF;
107 tracker_period = TRACKER_PERIOD;
111 bool PitchTracker::setParameters(
int priority,
int policy,
int sampleRate,
int buffersize) {
112 assert(buffersize <= FFT_SIZE);
118 m_sampleRate = fixed_sampleRate / DOWNSAMPLE;
119 resamp.setup(sampleRate, m_sampleRate, 1, 16);
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);
134 if (!m_fftwPlanFFT || !m_fftwPlanIFFT) {
141 start_thread(priority, policy);
148 pthread_cancel (m_pthr);
149 pthread_join (m_pthr, NULL);
154 void PitchTracker::start_thread(
int priority,
int policy) {
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);
166 if (pthread_create(&m_pthr, &attr, static_run,
167 reinterpret_cast<void*>(
this))) {
169 if (errno == EPERM) {
172 _(
"no permission to create realtime thread - please check your system configuration - tuner not started"));
176 _(
"error creating realtime thread - tuner not started"));
179 pthread_attr_destroy(&attr);
183 setParameters(priority, policy, samplerate, FFT_SIZE);
197 resamp.inp_count = count;
198 resamp.inp_data = input;
200 resamp.out_data = &m_buffer[m_bufferIndex];
201 int n = FFT_SIZE - m_bufferIndex;
202 resamp.out_count = n;
204 n -= resamp.out_count;
208 m_bufferIndex = (m_bufferIndex + n) % FFT_SIZE;
209 if (resamp.inp_count == 0) {
213 if (++tick * count >= m_sampleRate * DOWNSAMPLE * tracker_period) {
224 void PitchTracker::copy() {
225 int start = (FFT_SIZE + m_bufferIndex - m_buffersize) % FFT_SIZE;
226 int end = (FFT_SIZE + m_bufferIndex) % FFT_SIZE;
229 cnt = FFT_SIZE -
start;
230 memcpy(m_input, &m_buffer[
start], cnt *
sizeof(*m_input));
233 memcpy(&m_input[cnt], &m_buffer[
start], (
end -
start) *
sizeof(*m_input));
236 inline float sq(
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);
250 static int findMaxima(
float *input,
int len,
int *maxPositions,
int *length,
int maxLen) {
253 int overallMaxIndex = 0;
255 while (pos < (len-1)/3 && input[pos] > 0.0) {
258 while (pos < len-1 && input[pos] <= 0.0) {
264 while (pos < len-1) {
265 if (input[pos] > input[pos-1] && input[pos] >= input[pos+1]) {
266 if (curMaxPos == 0) {
268 }
else if (input[pos] > input[curMaxPos]) {
273 if (pos < len-1 && input[pos] <= 0.0) {
275 maxPositions[*length] = curMaxPos;
277 if (overallMaxIndex == 0) {
278 overallMaxIndex = curMaxPos;
279 }
else if (input[curMaxPos] > input[overallMaxIndex]) {
280 overallMaxIndex = curMaxPos;
282 if (*length >= maxLen) {
283 return overallMaxIndex;
287 while (pos < len-1 && input[pos] <= 0.0) {
293 maxPositions[*length] = curMaxPos;
295 if (overallMaxIndex == 0) {
296 overallMaxIndex = curMaxPos;
297 }
else if (input[curMaxPos] > input[overallMaxIndex]) {
298 overallMaxIndex = curMaxPos;
302 return overallMaxIndex;
305 static int findsubMaximum(
float *input,
int len,
float threshold) {
308 int overallMaxIndex = findMaxima(input, len, indices, &length, 10);
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) {
323 void PitchTracker::run() {
331 for (
int k = 0; k < m_buffersize; ++k) {
332 sum += fabs(m_input[k]);
334 float threshold = (m_audioLevel ? signal_threshold_off : signal_threshold_on);
335 m_audioLevel = (sum / m_buffersize >= threshold);
336 if ( m_audioLevel ==
false ) {
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;
351 m_fftwBufferFreq[0] =
sq(m_fftwBufferFreq[0]);
352 m_fftwBufferFreq[m_fftSize/2] =
sq(m_fftwBufferFreq[m_fftSize/2]);
354 fftwf_execute(m_fftwPlanIFFT);
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);
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]);
366 m_fftwBufferTime[k] *= 2.0 / sumSq;
368 m_fftwBufferTime[k] = 0.0;
371 const float thres = 0.99;
372 int maxAutocorrIndex = findsubMaximum(m_fftwBufferTime, count, thres);
375 if (maxAutocorrIndex >= 0) {
377 m_fftwBufferTime[maxAutocorrIndex],
378 m_fftwBufferTime[maxAutocorrIndex+1],
379 maxAutocorrIndex+1, &x);
380 x = m_sampleRate / x;
393 return m_freq <= 0.0 ? 1000.0 : 12 * log2f(2.272727e-03f * m_freq);