teensy-audio-tuner/AudioTuner.cpp

/* Audio Library Note Frequency Detection & Guitar/Bass Tuner
 * Copyright (c) 2015, Colin Duffy
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice, development funding notice, and this permission
 * notice shall be included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

#include "utility/dspinst.h"
#include "arm_math.h"
#include "Arduino.h"
#include "AudioTuner.h"

#define NUM_SAMPLES ( AUDIO_GUITARTUNER_BLOCKS << 7 )

void AudioTuner::update( void ) {
    
    audio_block_t *block;
    
    block = receiveReadOnly();
    if (!block) return;
    
    if ( !enabled ) {
        release( block );
        return;
    }
    
    /**
     *  "factor" is the new block size calculated by
     *  the decimated shift to incremnt the buffer
     *  address.
     */
    const uint8_t factor = AUDIO_BLOCK_SAMPLES >> decimation_shift;
    
    // filter and decimate block by block the incoming signal and store in a buffer.
    arm_fir_decimate_fast_q15( &firDecimateInst, block->data, AudioBuffer + ( state * factor ), AUDIO_BLOCK_SAMPLES );
    
    /**
     *  when half the blocks + 1 of the total
     *  blocks have been stored in the buffer
     *  start processing the data.
     */
    if ( state++ >= AUDIO_GUITARTUNER_BLOCKS >> 1 ) {
        
        if ( process_buffer ) process( AudioBuffer );
        
        if ( state == 0 ) process_buffer = true;
    }
    
    release( block );
}

/**
 *  Start the Yin algorithm
 *
 *  TODO: Significant speed up would be to use spectral domain to find fundamental frequency.
 *  This paper explains: https://aubio.org/phd/thesis/brossier06thesis.pdf -> Section 3.2.4
 *  page 79. Might have to downsample for low fundmental frequencies because of fft buffer
 *  size limit.
 */
void AudioTuner::process( int16_t *p ) {
    
    const uint16_t inner_cycles = ( NUM_SAMPLES >> decimation_shift ) >> 1;
    uint16_t outer_cycles = inner_cycles / AUDIO_GUITARTUNER_BLOCKS;
    uint16_t tau = tau_global;
    do {
        uint64_t sum = 0;
        int32_t  a1, a2, b1, b2, c1, c2, d1, d2;
        int32_t  out1, out2, out3, out4;
        uint16_t blkCnt;
        int16_t * cur = p;
        int16_t * lag = p + tau;
        // unrolling the inner loop by 8
        blkCnt = inner_cycles >> 3;
        do {
            // a(n), b(n), c(n), d(n) each hold two samples
            a1 = *__SIMD32( cur )++;
            a2 = *__SIMD32( cur )++;
            b1 = *__SIMD32( lag )++;
            b2 = *__SIMD32( lag )++;
            c1 = *__SIMD32( cur )++;
            c2 = *__SIMD32( cur )++;
            d1 = *__SIMD32( lag )++;
            d2 = *__SIMD32( lag )++;
            // subract two samples at a time
            out1 = __QSUB16( a1, b1 );
            out2 = __QSUB16( a2, b2 );
            out3 = __QSUB16( c1, d1 );
            out4 = __QSUB16( c2, d2 );
            // square the difference
            sum = multiply_accumulate_16tx16t_add_16bx16b( sum, out1, out1 );
            sum = multiply_accumulate_16tx16t_add_16bx16b( sum, out2, out2 );
            sum = multiply_accumulate_16tx16t_add_16bx16b( sum, out3, out3 );
            sum = multiply_accumulate_16tx16t_add_16bx16b( sum, out4, out4 );
            
        } while( --blkCnt );
        
        uint64_t rs = running_sum;
        rs += sum;
        yin_buffer[yin_idx] = sum*tau;
        rs_buffer[yin_idx] = rs;
        running_sum = rs;
        yin_idx = ( ++yin_idx >= 5 ) ? 0 : yin_idx;
        tau = estimate( yin_buffer, rs_buffer, yin_idx, tau );
        
        if ( tau == 0 ) {
            process_buffer  = false;
            new_output      = true;
            yin_idx         = 1;
            running_sum     = 0;
            tau_global      = 1;
            state           = 0;
            return;
        }
        
    } while ( --outer_cycles );
    
    if ( tau >= inner_cycles ) {
        process_buffer  = true;
        new_output      = false;
        yin_idx         = 1;
        running_sum     = 0;
        tau_global      = 1;
        state           = 0;
        return;
    }
    tau_global = tau;
}

/**
 *  check the sampled data for fundamental frequency
 *
 *  @param yin  buffer to hold sum*tau value
 *  @param rs   buffer to hold running sum for sampled window
 *  @param head buffer index
 *  @param tau  lag we are curly working on gets incremented
 *
 *  @return tau
 */
uint16_t AudioTuner::estimate( uint64_t *yin, uint64_t *rs, uint16_t head, uint16_t tau ) {
    const uint64_t *y = ( uint64_t * )yin;
    const uint64_t *r = ( uint64_t * )rs;
    uint16_t _tau, _head;
    const float thresh = yin_threshold;
    _tau = tau;
    _head = head;
    
    if ( _tau > 4 ) {
        
        uint16_t idx0, idx1, idx2;
        idx0 = _head;
        idx1 = _head + 1;
        idx1 = ( idx1 >= 5 ) ? 0 : idx1;
        idx2 = _head + 2;
        idx2 = ( idx2 >= 5 ) ? idx2 - 5 : idx2;
        
        // maybe fixed point would be better here? But how?
        float s0, s1, s2;
        s0 = ( ( float )*( y+idx0 ) / ( float )*( r+idx0 ) );
        s1 = ( ( float )*( y+idx1 ) / ( float )*( r+idx1 ) );
        s2 = ( ( float )*( y+idx2 ) / ( float )*( r+idx2 ) );
        
        if ( s1 < thresh && s1 < s2 ) {
            uint16_t period = _tau - 3;
            periodicity = 1 - s1;
            data = period + 0.5f * ( s0 - s2 ) / ( s0 - 2.0f * s1 + s2 );
            return 0;
        }
    }
    return _tau + 1;
}

/**
 *  Initialise
 *
 *  @param threshold Allowed uncertainty
 */
void AudioTuner::begin( float threshold, int16_t *coeff, uint8_t taps, uint8_t factor ) {
    __disable_irq( );
    process_buffer      = true;
    yin_threshold       = threshold;
    periodicity         = 0.0f;
    running_sum         = 0;
    tau_global          = 1;
    yin_idx             = 1;
    enabled             = true;
    state               = 0;
    data                = 0.0f;
    decimation_factor   = factor;
    decimation_shift    = log( factor ) / log( 2 );
    coeff_size          = taps;
    coeff_p             = coeff;
    arm_fir_decimate_init_q15( &firDecimateInst, coeff_size, decimation_factor, coeff_p, &coeff_state[0], AUDIO_BLOCK_SAMPLES );
    __enable_irq( );
}

/**
 *  available
 *
 *  @return true if data is ready else false
 */
bool AudioTuner::available( void ) {
    __disable_irq( );
    bool flag = new_output;
    if ( flag ) new_output = false;
    __enable_irq( );
    return flag;
}

/**
 *  read processes the data samples for the Yin algorithm.
 *
 *  @return frequency in hertz
 */
float AudioTuner::read( void ) {
    __disable_irq( );
    float d = data;
    __enable_irq( );
    return ( AUDIO_SAMPLE_RATE_EXACT / decimation_factor ) / d;
}

/**
 *  Periodicity of the sampled signal.
 *
 *  @return periodicity
 */
float AudioTuner::probability( void ) {
    __disable_irq( );
    float p = periodicity;
    __enable_irq( );
    return p;
}

/**
 *  New LP coeffients for decimation.
 *
 *  @param p    array pointer of coeffients.
 *  @param n    array size.
 */
void AudioTuner::coeff( int16_t *p, int n ) {
    //coeff_size = n;
    //coeff_p = p;
    //arm_fir_decimate_init_q15(&firDecimateInst, coeff_size, 4, coeff_p, coeff_state, 128);
}

/**
 *  Initialise parameters.
 *
 *  @param thresh    Allowed uncertainty
 */
void AudioTuner::threshold( float p ) {
    __disable_irq( );
    yin_threshold = p;
    __enable_irq( );
}

/**
 *  disable yin from processing data, use begin to start back up
 *
 *  @return none
 */
void AudioTuner::disable( void ) {
    __disable_irq( );
    enabled = false;
    __enable_irq( );
}