Logo Search packages:      
Sourcecode: guitarix version File versions  Download package

gx_pitch_tracker.cpp

/*
 * Copyright (C) 2009, 2010 Hermann Meyer, James Warden, Andreas Degert
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 * --------------------------------------------------------------------------
 */

/* ------- This is the guitarix tuner, part of gx_engine_audio.cpp ------- */

#include <cstring>
#include <jack/jack.h>
#include "guitarix.h"

/****************************************************************
 ** Pitch Tracker
 **
 ** some code and ideas taken from K4Guitune (William Spinelli)
 **
 */

namespace gx_engine {

const int DOWNSAMPLE = 16; // downsampling factor
const int ZERO_PADDING_FACTOR = 64; // Number of times that the FFT is zero-padded to increase frequency resolution.
const float SIGNAL_THRESHOLD_ON = 0.001; // Value of the threshold above which the processing is activated.
const float SIGNAL_THRESHOLD_OFF = 0.0009; // Value of the threshold below which the input audio signal is deactivated.
const float TRACKER_PERIOD = 0.1; // Time between frequency estimates (in seconds)

void *PitchTracker::static_run(void *p)
{
      ((PitchTracker *)p)->run();
      return NULL;
}

PitchTracker::PitchTracker():
      error(false),
      busy(false),
      tick(0),
      m_pthr(0),
      m_buffer(new float[MAX_FFT_SIZE]),
      m_bufferIndex(0),
      m_audioLevel(false),
      m_fftwPlanFFT(0),
      m_fftwPlanIFFT(0)
{
      const int fftw_buffer_size = MAX_FFT_SIZE * ZERO_PADDING_FACTOR;
      m_fftwBufferTime = (float*)fftwf_malloc(fftw_buffer_size * sizeof(float));
      m_fftwBufferFreq = (fftwf_complex*)fftwf_malloc(fftw_buffer_size * sizeof(fftwf_complex));

      memset(m_buffer, 0, MAX_FFT_SIZE * sizeof(float));
      memset(m_fftwBufferTime, 0, fftw_buffer_size * sizeof(float));
      memset(m_fftwBufferFreq, 0, fftw_buffer_size * sizeof(fftwf_complex));

    sem_init(&m_trig, 0, 0);

      if (!m_buffer || !m_fftwBufferTime || !m_fftwBufferFreq) {
            error = true;
      }
}


PitchTracker::~PitchTracker()
{
      fftwf_destroy_plan(m_fftwPlanFFT);
      fftwf_destroy_plan(m_fftwPlanIFFT);
      fftwf_free(m_fftwBufferTime);
      fftwf_free(m_fftwBufferFreq);
      delete[] m_buffer;
}


bool PitchTracker::setParameters(int sampleRate, int fftSize)
{
      assert(fftSize <= MAX_FFT_SIZE);

      if (error) {
            return false;
      }
      m_sampleRate = sampleRate / DOWNSAMPLE;
      resamp.setup(sampleRate, m_sampleRate, 1, 16); // 16 == least quality

      if (m_fftSize != fftSize) {
            m_fftSize = fftSize;
            fftwf_destroy_plan(m_fftwPlanFFT);
            fftwf_destroy_plan(m_fftwPlanIFFT);
            m_fftwPlanFFT = fftwf_plan_dft_r2c_1d(
                  m_fftSize, m_fftwBufferTime, m_fftwBufferFreq, FFTW_ESTIMATE); // FFT
            m_fftwPlanIFFT = fftwf_plan_dft_c2r_1d(
                  ZERO_PADDING_FACTOR * m_fftSize, m_fftwBufferFreq, m_fftwBufferTime, FFTW_ESTIMATE); // IFFT zero-padded
      }

      if (!m_fftwPlanFFT || !m_fftwPlanIFFT) {
            error = true;
            return false;
      }

      if (!m_pthr) {
            start_thread();
      }
      pt_initialized = true;
      return !error;
}

void PitchTracker::start_thread()
{
    int                min, max;
    pthread_attr_t     attr;
    struct sched_param  spar;
    int priority, policy;
    pthread_getschedparam(jack_client_thread_id(gx_jack::client), &policy, &spar);
    priority = spar.sched_priority;
    min = sched_get_priority_min(policy);
    max = sched_get_priority_max(policy);
    priority -= 6; // zita-convoler uses 5 levels
    if (priority > max) priority = max;
    if (priority < min) priority = min;
    spar.sched_priority = priority;
    pthread_attr_init (&attr);
    pthread_attr_setdetachstate (&attr, PTHREAD_CREATE_DETACHED);
    pthread_attr_setschedpolicy (&attr, policy);
    pthread_attr_setschedparam (&attr, &spar);
    pthread_attr_setscope (&attr, PTHREAD_SCOPE_SYSTEM);
    pthread_attr_setinheritsched (&attr, PTHREAD_EXPLICIT_SCHED);
    //pthread_attr_setstacksize (&attr, 0x10000);
    if (pthread_create (&m_pthr, &attr, static_run, (void*)this)) {
          error = true;
    }
    pthread_attr_destroy (&attr);
}

int PitchTracker::find_minimum()
{
      const int peakwidth = 3;
      float *p = &m_fftwBufferTime[peakwidth];
      for ( ; p < &m_fftwBufferTime[ZERO_PADDING_FACTOR * m_fftSize / 2 + 1 - peakwidth]; p++) {
            int i;
            for (i = -peakwidth; i <= peakwidth; i++) {
                  if (*p > p[i]) {
                        break;
                  }
            }
            if (i > peakwidth) {
                  break;
            }
      }
      return (int)(p - m_fftwBufferTime);
}

int PitchTracker::find_maximum(int l)
{
      float maxAutocorr             = 0.0;
      int         maxAutocorrIndex  = 0;
      while ( l < ZERO_PADDING_FACTOR * m_fftSize / 2 + 1) {
            if (m_fftwBufferTime[l] > maxAutocorr) {
                  maxAutocorr = m_fftwBufferTime[l];
                  maxAutocorrIndex = l;
            }
            ++l;
      }
      if (maxAutocorr == 0.0) {
            return -1;
      }
      return maxAutocorrIndex;
}

float show_level(int n, float *buf)
{
      float sum = 0.0;
      for (int k = 0; k < n; ++k) {
            sum += fabs(buf[k]);
      }
      return sum;
}

void PitchTracker::add(int count, float* input)
{
      if (error) {
            return;
      }
      resamp.inp_count = count;
      resamp.inp_data = input;
      for (;;) {
            resamp.out_data = &m_buffer[m_bufferIndex];
            int n = MAX_FFT_SIZE - m_bufferIndex;
            resamp.out_count = n;
            resamp.process();
            n -= resamp.out_count; // n := number of output samples
            if (!n) { // all soaked up by filter
                  return;
            }
            m_bufferIndex = (m_bufferIndex + n) % MAX_FFT_SIZE;
            if (resamp.inp_count == 0) {
                  break;
            }
      }
      if (++tick * count >= m_sampleRate * DOWNSAMPLE * TRACKER_PERIOD) {
            if (busy) {
                  return;
            }
            tick = 0;
            copy();
            sem_post(&m_trig);
      }
}

void PitchTracker::copy()
{
      int start = (MAX_FFT_SIZE + m_bufferIndex - m_fftSize) % MAX_FFT_SIZE;
      int end = (MAX_FFT_SIZE + m_bufferIndex) % MAX_FFT_SIZE;
      int cnt = 0;
      if (start >= end) {
            cnt = MAX_FFT_SIZE - start;
            memcpy(m_fftwBufferTime, &m_buffer[start], cnt * sizeof(float));
            start = 0;
      }
      memcpy(&m_fftwBufferTime[cnt], &m_buffer[start], (end - start) * sizeof(float));
}

void PitchTracker::run()
{
      for (;;) {
            busy = false;
            sem_wait(&m_trig);
            busy = true;
            if (error) {
                  continue;
            }
            float sum = 0.0;
            for (int k = 0; k < m_fftSize; ++k) {
                  sum += fabs(m_fftwBufferTime[k]);
            }
            float threshold = (m_audioLevel ? SIGNAL_THRESHOLD_OFF : SIGNAL_THRESHOLD_ON);
            m_audioLevel = (sum / m_fftSize >= threshold);
            if ( m_audioLevel == false ) {
                  setEstimatedFrequency(0.0);
                  continue;
            }

            /* Compute the transform of the autocorrelation given in time domain by
             *           k=-N
             *    r[t] = sum( x[k] * x[t-k] )
             *            N
             * or in the frequency domain (for a real signal) by
             *    R[f] = X[f] * X[f]' = |X[f]|^2 = Re(X[f])^2 + Im(X[f])^2
             * When computing the FFT with fftwf_plan_dft_r2c_1d there are only N/2+1
             * significant samples, so |.|^2 is computed for m_fftSize/2+1 samples only
             */
            int fftRSize = m_fftSize/2 + 1;
            fftwf_execute(m_fftwPlanFFT);
            for (int k = 0; k < fftRSize; ++k) {
                  fftwf_complex& v = m_fftwBufferFreq[k];
                  v[0] = v[0]*v[0] + v[1]*v[1];
                  v[1] = 0.0;
            }

            // pad the FFT with zeros to increase resolution in time domain after IFFT
            int size_with_padding = ZERO_PADDING_FACTOR * m_fftSize - fftRSize;
            memset(&m_fftwBufferFreq[fftRSize][0], 0, size_with_padding * sizeof(fftwf_complex));
            fftwf_execute(m_fftwPlanIFFT);

            // search for a minimum and then for the next maximum to get the estimated frequency
            int maxAutocorrIndex = find_maximum(find_minimum());

            // compute the frequency of the maximum considering the padding factor
            if (maxAutocorrIndex >= 0) {
                  setEstimatedFrequency(ZERO_PADDING_FACTOR * m_sampleRate / (float)maxAutocorrIndex);
            } else {
                  setEstimatedFrequency(0.0);
            }
            busy = false;
      }
}

void PitchTracker::setEstimatedFrequency(float freq)
{
      midi.fConsta4 = freq;
      audio.fConsta1t = (freq == 0.0 ? 1000.0 : 12 * log2f(2.272727e-03f * freq));
}

PitchTracker pitch_tracker;

}



Generated by  Doxygen 1.6.0   Back to index