|
|
|
@@ -0,0 +1,267 @@ |
|
|
|
/* Audio Library Guitar and 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 "analyze_guitartuner.h" |
|
|
|
#include "utility/dspinst.h" |
|
|
|
#include "arm_math.h" |
|
|
|
|
|
|
|
#define HALF_BLOCKS AUDIO_BLOCKS * 64 |
|
|
|
|
|
|
|
#define LOOP1(a) a |
|
|
|
#define LOOP2(a) a LOOP1(a) |
|
|
|
#define LOOP3(a) a LOOP2(a) |
|
|
|
#define LOOP4(a) a LOOP3(a) |
|
|
|
#define LOOP8(a) a LOOP3(a) a LOOP3(a) |
|
|
|
#define LOOP16(a) a LOOP8(a) a LOOP2(a) a LOOP3(a) |
|
|
|
#define LOOP32(a) a LOOP16(a) a LOOP8(a) a LOOP1(a) a LOOP3(a) |
|
|
|
#define LOOP64(a) a LOOP32(a) a LOOP16(a) a LOOP8(a) a LOOP2(a) a LOOP1(a) |
|
|
|
#define UNROLL(n,a) LOOP##n(a) |
|
|
|
|
|
|
|
static void copy_buffer(void *destination, const void *source) { |
|
|
|
const uint16_t *src = (const uint16_t *)source; |
|
|
|
uint16_t *dst = (uint16_t *)destination; |
|
|
|
for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) *dst++ = *src++; |
|
|
|
} |
|
|
|
|
|
|
|
void AudioTuner::update( void ) { |
|
|
|
|
|
|
|
audio_block_t *block; |
|
|
|
|
|
|
|
block = receiveReadOnly(); |
|
|
|
if (!block) return; |
|
|
|
|
|
|
|
if ( !enabled ) { |
|
|
|
release( block ); |
|
|
|
return; |
|
|
|
} |
|
|
|
|
|
|
|
digitalWriteFast(2, HIGH); |
|
|
|
if ( next_buffer ) { |
|
|
|
blocklist1[state++] = block; |
|
|
|
if ( !first_run && process_buffer ) process( ); |
|
|
|
} else { |
|
|
|
blocklist2[state++] = block; |
|
|
|
if ( !first_run && process_buffer ) process( ); |
|
|
|
} |
|
|
|
|
|
|
|
if ( state >= AUDIO_BLOCKS ) { |
|
|
|
if ( next_buffer ) { |
|
|
|
if ( !first_run && process_buffer ) process( ); |
|
|
|
for ( int i = 0; i < AUDIO_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist1[i]->data ); |
|
|
|
for ( int i = 0; i < AUDIO_BLOCKS; i++ ) release(blocklist1[i] ); |
|
|
|
} else { |
|
|
|
if ( !first_run && process_buffer ) process( ); |
|
|
|
for ( int i = 0; i < AUDIO_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist2[i]->data ); |
|
|
|
for ( int i = 0; i < AUDIO_BLOCKS; i++ ) release( blocklist2[i] ); |
|
|
|
} |
|
|
|
process_buffer = true; |
|
|
|
first_run = false; |
|
|
|
state = 0; |
|
|
|
//digitalWriteFast(LED_BUILTIN, !digitalReadFast(LED_BUILTIN)); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
FASTRUN void AudioTuner::process( void ) { |
|
|
|
//digitalWriteFast(0, HIGH); |
|
|
|
|
|
|
|
const int16_t *p; |
|
|
|
p = AudioBuffer; |
|
|
|
|
|
|
|
uint16_t cycles = 64;; |
|
|
|
uint16_t tau = tau_global; |
|
|
|
do { |
|
|
|
uint16_t x = 0; |
|
|
|
int64_t sum = 0; |
|
|
|
//uint32_t res; |
|
|
|
do { |
|
|
|
/*int16_t current1, lag1, current2, lag2; |
|
|
|
int32_t val1, val2; |
|
|
|
lag1 = *( ( uint32_t * )p + ( x + tau ) ); |
|
|
|
current1 = *( ( uint32_t * )p + x ); |
|
|
|
x += 32; |
|
|
|
lag2 = *( ( uint32_t * )p + ( x + tau ) ); |
|
|
|
current2 = *( ( uint32_t * )p + x ); |
|
|
|
val1 = __PKHBT(current1, current2, 0x10); |
|
|
|
val2 = __PKHBT(lag1, lag2, 0x10); |
|
|
|
res = __SSUB16( val1, val2 ); |
|
|
|
sum = __SMLALD(res, res, sum); |
|
|
|
//sum = __SMLSLD(delta1, delta2, sum);*/ |
|
|
|
int16_t current, lag, delta; |
|
|
|
//UNROLL(16, |
|
|
|
lag = *( ( int16_t * )p + ( x+tau ) ); |
|
|
|
current = *( ( int16_t * )p+x ); |
|
|
|
delta = ( current-lag ); |
|
|
|
sum += delta * delta; |
|
|
|
#if F_CPU == 144000000 |
|
|
|
x += 8; |
|
|
|
#elif F_CPU == 120000000 |
|
|
|
x += 12; |
|
|
|
#elif F_CPU == 96000000 |
|
|
|
x += 16; |
|
|
|
#elif F_CPU < 96000000 |
|
|
|
x += 32; |
|
|
|
#endif |
|
|
|
//); |
|
|
|
} while ( x <= HALF_BLOCKS ); |
|
|
|
|
|
|
|
running_sum += sum; |
|
|
|
yin_buffer[yin_idx] = sum*tau; |
|
|
|
rs_buffer[yin_idx] = running_sum; |
|
|
|
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; |
|
|
|
//digitalWriteFast(2, LOW); |
|
|
|
//digitalWriteFast(0, LOW); |
|
|
|
return; |
|
|
|
} |
|
|
|
} while ( --cycles ); |
|
|
|
|
|
|
|
if ( tau >= HALF_BLOCKS ) { |
|
|
|
process_buffer = false; |
|
|
|
new_output = false; |
|
|
|
yin_idx = 1; |
|
|
|
running_sum = 0; |
|
|
|
tau_global = 1; |
|
|
|
//digitalWriteFast(0, LOW); |
|
|
|
return; |
|
|
|
} |
|
|
|
tau_global = tau; |
|
|
|
//digitalWriteFast(0, LOW); |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* check the sampled data for fundmental 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 currently working on this gets incremented |
|
|
|
* |
|
|
|
* @return tau |
|
|
|
*/ |
|
|
|
uint16_t AudioTuner::estimate( int64_t *yin, int64_t *rs, uint16_t head, uint16_t tau ) { |
|
|
|
const int64_t *y = ( int64_t * )yin; |
|
|
|
const int64_t *r = ( int64_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 ) ? 0 : idx2; |
|
|
|
|
|
|
|
float s0, s1, s2; |
|
|
|
s0 = ( ( float )*( y+idx0 ) / *( r+idx0 ) ); |
|
|
|
s1 = ( ( float )*( y+idx1 ) / *( r+idx1 ) ); |
|
|
|
s2 = ( ( float )*( y+idx2 ) / *( 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 |
|
|
|
* @param cpu_max How much cpu usage before throttling |
|
|
|
*/ |
|
|
|
void AudioTuner::initialize( float threshold ) { |
|
|
|
__disable_irq( ); |
|
|
|
process_buffer = false; |
|
|
|
yin_threshold = threshold; |
|
|
|
periodicity = 0.0f; |
|
|
|
next_buffer = true; |
|
|
|
running_sum = 0; |
|
|
|
tau_global = 1; |
|
|
|
first_run = true; |
|
|
|
yin_idx = 1; |
|
|
|
enabled = true; |
|
|
|
state = 0; |
|
|
|
data = 0.0f; |
|
|
|
__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 / d; |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* Periodicity of the sampled signal from Yin algorithm from read function. |
|
|
|
* |
|
|
|
* @return periodicity |
|
|
|
*/ |
|
|
|
float AudioTuner::probability( void ) { |
|
|
|
__disable_irq( ); |
|
|
|
float p = periodicity; |
|
|
|
__enable_irq( ); |
|
|
|
return p; |
|
|
|
} |
|
|
|
|
|
|
|
/** |
|
|
|
* Initialise parameters. |
|
|
|
* |
|
|
|
* @param thresh Allowed uncertainty |
|
|
|
*/ |
|
|
|
void AudioTuner::threshold( float p ) { |
|
|
|
__disable_irq( ); |
|
|
|
yin_threshold = p; |
|
|
|
__enable_irq( ); |
|
|
|
} |
PaulStoffregen
9 년 전