/* 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( ); }