268 lines
7.8KB

  1. /* Audio Library Note Frequency Detection & Guitar/Bass Tuner
  2. * Copyright (c) 2015, Colin Duffy
  3. *
  4. * Permission is hereby granted, free of charge, to any person obtaining a copy
  5. * of this software and associated documentation files (the "Software"), to deal
  6. * in the Software without restriction, including without limitation the rights
  7. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  8. * copies of the Software, and to permit persons to whom the Software is
  9. * furnished to do so, subject to the following conditions:
  10. *
  11. * The above copyright notice, development funding notice, and this permission
  12. * notice shall be included in all copies or substantial portions of the Software.
  13. *
  14. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  15. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  16. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  17. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  18. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  19. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  20. * THE SOFTWARE.
  21. */
  22. #include <Arduino.h>
  23. #include "analyze_notefreq.h"
  24. #include "utility/dspinst.h"
  25. #include "arm_math.h"
  26. #define HALF_BLOCKS AUDIO_GUITARTUNER_BLOCKS * 64
  27. /**
  28. * Copy internal blocks of data to class buffer
  29. *
  30. * @param destination destination address
  31. * @param source source address
  32. */
  33. static void copy_buffer(void *destination, const void *source) {
  34. const uint16_t *src = ( const uint16_t * )source;
  35. uint16_t *dst = ( uint16_t * )destination;
  36. for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) *dst++ = (*src++);
  37. }
  38. /**
  39. * Virtual function to override from Audio Library
  40. */
  41. void AudioAnalyzeNoteFrequency::update( void ) {
  42. audio_block_t *block;
  43. block = receiveReadOnly();
  44. if (!block) return;
  45. if ( !enabled ) {
  46. release( block );
  47. return;
  48. }
  49. if ( next_buffer ) {
  50. blocklist1[state++] = block;
  51. if ( !first_run && process_buffer ) process( );
  52. } else {
  53. blocklist2[state++] = block;
  54. if ( !first_run && process_buffer ) process( );
  55. }
  56. if ( state >= AUDIO_GUITARTUNER_BLOCKS ) {
  57. if ( next_buffer ) {
  58. if ( !first_run && process_buffer ) process( );
  59. for ( int i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist1[i]->data );
  60. for ( int i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) release( blocklist1[i] );
  61. next_buffer = false;
  62. } else {
  63. if ( !first_run && process_buffer ) process( );
  64. for ( int i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) copy_buffer( AudioBuffer+( i * 0x80 ), blocklist2[i]->data );
  65. for ( int i = 0; i < AUDIO_GUITARTUNER_BLOCKS; i++ ) release( blocklist2[i] );
  66. next_buffer = true;
  67. }
  68. process_buffer = true;
  69. first_run = false;
  70. state = 0;
  71. }
  72. }
  73. /**
  74. * Start the Yin algorithm
  75. *
  76. * TODO: Significant speed up would be to use spectral domain to find fundamental frequency.
  77. * This paper explains: https://aubio.org/phd/thesis/brossier06thesis.pdf -> Section 3.2.4
  78. * page 79. Might have to downsample for low fundmental frequencies because of fft buffer
  79. * size limit.
  80. */
  81. void AudioAnalyzeNoteFrequency::process( void ) {
  82. const int16_t *p;
  83. p = AudioBuffer;
  84. uint16_t cycles = 64;
  85. uint16_t tau = tau_global;
  86. do {
  87. uint16_t x = 0;
  88. uint64_t sum = 0;
  89. do {
  90. int16_t current, lag, delta;
  91. lag = *( ( int16_t * )p + ( x+tau ) );
  92. current = *( ( int16_t * )p+x );
  93. delta = ( current-lag );
  94. sum += delta * delta;
  95. x += 4;
  96. lag = *( ( int16_t * )p + ( x+tau ) );
  97. current = *( ( int16_t * )p+x );
  98. delta = ( current-lag );
  99. sum += delta * delta;
  100. x += 4;
  101. lag = *( ( int16_t * )p + ( x+tau ) );
  102. current = *( ( int16_t * )p+x );
  103. delta = ( current-lag );
  104. sum += delta * delta;
  105. x += 4;
  106. lag = *( ( int16_t * )p + ( x+tau ) );
  107. current = *( ( int16_t * )p+x );
  108. delta = ( current-lag );
  109. sum += delta * delta;
  110. x += 4;
  111. } while ( x < HALF_BLOCKS );
  112. uint64_t rs = running_sum;
  113. rs += sum;
  114. yin_buffer[yin_idx] = sum*tau;
  115. rs_buffer[yin_idx] = rs;
  116. running_sum = rs;
  117. yin_idx = ( ++yin_idx >= 5 ) ? 0 : yin_idx;
  118. tau = estimate( yin_buffer, rs_buffer, yin_idx, tau );
  119. if ( tau == 0 ) {
  120. process_buffer = false;
  121. new_output = true;
  122. yin_idx = 1;
  123. running_sum = 0;
  124. tau_global = 1;
  125. return;
  126. }
  127. } while ( --cycles );
  128. //digitalWriteFast(10, LOW);
  129. if ( tau >= HALF_BLOCKS ) {
  130. process_buffer = false;
  131. new_output = false;
  132. yin_idx = 1;
  133. running_sum = 0;
  134. tau_global = 1;
  135. return;
  136. }
  137. tau_global = tau;
  138. }
  139. /**
  140. * check the sampled data for fundamental frequency
  141. *
  142. * @param yin buffer to hold sum*tau value
  143. * @param rs buffer to hold running sum for sampled window
  144. * @param head buffer index
  145. * @param tau lag we are currently working on gets incremented
  146. *
  147. * @return tau
  148. */
  149. uint16_t AudioAnalyzeNoteFrequency::estimate( uint64_t *yin, uint64_t *rs, uint16_t head, uint16_t tau ) {
  150. const uint64_t *y = ( uint64_t * )yin;
  151. const uint64_t *r = ( uint64_t * )rs;
  152. uint16_t _tau, _head;
  153. const float thresh = yin_threshold;
  154. _tau = tau;
  155. _head = head;
  156. if ( _tau > 4 ) {
  157. uint16_t idx0, idx1, idx2;
  158. idx0 = _head;
  159. idx1 = _head + 1;
  160. idx1 = ( idx1 >= 5 ) ? 0 : idx1;
  161. idx2 = head + 2;
  162. idx2 = ( idx2 >= 5 ) ? 0 : idx2;
  163. float s0, s1, s2;
  164. s0 = ( ( float )*( y+idx0 ) / *( r+idx0 ) );
  165. s1 = ( ( float )*( y+idx1 ) / *( r+idx1 ) );
  166. s2 = ( ( float )*( y+idx2 ) / *( r+idx2 ) );
  167. if ( s1 < thresh && s1 < s2 ) {
  168. uint16_t period = _tau - 3;
  169. periodicity = 1 - s1;
  170. data = period + 0.5f * ( s0 - s2 ) / ( s0 - 2.0f * s1 + s2 );
  171. return 0;
  172. }
  173. }
  174. return _tau + 1;
  175. }
  176. /**
  177. * Initialise
  178. *
  179. * @param threshold Allowed uncertainty
  180. */
  181. void AudioAnalyzeNoteFrequency::begin( float threshold ) {
  182. __disable_irq( );
  183. process_buffer = false;
  184. yin_threshold = threshold;
  185. periodicity = 0.0f;
  186. next_buffer = true;
  187. running_sum = 0;
  188. tau_global = 1;
  189. first_run = true;
  190. yin_idx = 1;
  191. enabled = true;
  192. state = 0;
  193. data = 0.0f;
  194. __enable_irq( );
  195. }
  196. /**
  197. * available
  198. *
  199. * @return true if data is ready else false
  200. */
  201. bool AudioAnalyzeNoteFrequency::available( void ) {
  202. __disable_irq( );
  203. bool flag = new_output;
  204. if ( flag ) new_output = false;
  205. __enable_irq( );
  206. return flag;
  207. }
  208. /**
  209. * read processes the data samples for the Yin algorithm.
  210. *
  211. * @return frequency in hertz
  212. */
  213. float AudioAnalyzeNoteFrequency::read( void ) {
  214. __disable_irq( );
  215. float d = data;
  216. __enable_irq( );
  217. return AUDIO_SAMPLE_RATE_EXACT / d;
  218. }
  219. /**
  220. * Periodicity of the sampled signal from Yin algorithm from read function.
  221. *
  222. * @return periodicity
  223. */
  224. float AudioAnalyzeNoteFrequency::probability( void ) {
  225. __disable_irq( );
  226. float p = periodicity;
  227. __enable_irq( );
  228. return p;
  229. }
  230. /**
  231. * Initialise parameters.
  232. *
  233. * @param thresh Allowed uncertainty
  234. */
  235. void AudioAnalyzeNoteFrequency::threshold( float p ) {
  236. __disable_irq( );
  237. yin_threshold = p;
  238. __enable_irq( );
  239. }