Nevar pievienot vairāk kā 25 tēmas Tēmai ir jāsākas ar burtu vai ciparu, tā var saturēt domu zīmes ('-') un var būt līdz 35 simboliem gara.

131 rinda
4.0KB

  1. /* Audio Library for Teensy 3.X
  2. * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com
  3. *
  4. * Development of this audio library was funded by PJRC.COM, LLC by sales of
  5. * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop
  6. * open source software by purchasing Teensy or other PJRC products.
  7. *
  8. * Permission is hereby granted, free of charge, to any person obtaining a copy
  9. * of this software and associated documentation files (the "Software"), to deal
  10. * in the Software without restriction, including without limitation the rights
  11. * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
  12. * copies of the Software, and to permit persons to whom the Software is
  13. * furnished to do so, subject to the following conditions:
  14. *
  15. * The above copyright notice, development funding notice, and this permission
  16. * notice shall be included in all copies or substantial portions of the Software.
  17. *
  18. * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  19. * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  20. * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  21. * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  22. * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  23. * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  24. * THE SOFTWARE.
  25. */
  26. #include <Arduino.h>
  27. #include "analyze_fft256.h"
  28. #include "sqrt_integer.h"
  29. #include "utility/dspinst.h"
  30. // 140312 - PAH - slightly faster copy
  31. static void copy_to_fft_buffer(void *destination, const void *source)
  32. {
  33. const uint16_t *src = (const uint16_t *)source;
  34. uint32_t *dst = (uint32_t *)destination;
  35. for (int i=0; i < AUDIO_BLOCK_SAMPLES; i++) {
  36. *dst++ = *src++; // real sample plus a zero for imaginary
  37. }
  38. }
  39. static void apply_window_to_fft_buffer(void *buffer, const void *window)
  40. {
  41. int16_t *buf = (int16_t *)buffer;
  42. const int16_t *win = (int16_t *)window;;
  43. for (int i=0; i < 256; i++) {
  44. int32_t val = *buf * *win++;
  45. //*buf = signed_saturate_rshift(val, 16, 15);
  46. *buf = val >> 15;
  47. buf += 2;
  48. }
  49. }
  50. void AudioAnalyzeFFT256::update(void)
  51. {
  52. audio_block_t *block;
  53. block = receiveReadOnly();
  54. if (!block) return;
  55. #if AUDIO_BLOCK_SAMPLES == 128
  56. if (!prevblock) {
  57. prevblock = block;
  58. return;
  59. }
  60. copy_to_fft_buffer(buffer, prevblock->data);
  61. copy_to_fft_buffer(buffer+256, block->data);
  62. //window = AudioWindowBlackmanNuttall256;
  63. //window = NULL;
  64. if (window) apply_window_to_fft_buffer(buffer, window);
  65. arm_cfft_radix4_q15(&fft_inst, buffer);
  66. // G. Heinzel's paper says we're supposed to average the magnitude
  67. // squared, then do the square root at the end.
  68. if (count == 0) {
  69. for (int i=0; i < 128; i++) {
  70. uint32_t tmp = *((uint32_t *)buffer + i);
  71. uint32_t magsq = multiply_16tx16t_add_16bx16b(tmp, tmp);
  72. sum[i] = magsq / naverage;
  73. }
  74. } else {
  75. for (int i=0; i < 128; i++) {
  76. uint32_t tmp = *((uint32_t *)buffer + i);
  77. uint32_t magsq = multiply_16tx16t_add_16bx16b(tmp, tmp);
  78. sum[i] += magsq / naverage;
  79. }
  80. }
  81. if (++count == naverage) {
  82. count = 0;
  83. for (int i=0; i < 128; i++) {
  84. output[i] = sqrt_uint32_approx(sum[i]);
  85. }
  86. outputflag = true;
  87. }
  88. release(prevblock);
  89. prevblock = block;
  90. #elif AUDIO_BLOCK_SAMPLES == 64
  91. if (prevblocks[2] == NULL) {
  92. prevblocks[2] = prevblocks[1];
  93. prevblocks[1] = prevblocks[0];
  94. prevblocks[0] = block;
  95. return;
  96. }
  97. if (count == 0) {
  98. count = 1;
  99. copy_to_fft_buffer(buffer, prevblocks[2]->data);
  100. copy_to_fft_buffer(buffer+128, prevblocks[1]->data);
  101. copy_to_fft_buffer(buffer+256, prevblocks[1]->data);
  102. copy_to_fft_buffer(buffer+384, block->data);
  103. if (window) apply_window_to_fft_buffer(buffer, window);
  104. arm_cfft_radix4_q15(&fft_inst, buffer);
  105. } else {
  106. count = 2;
  107. const uint32_t *p = (uint32_t *)buffer;
  108. for (int i=0; i < 128; i++) {
  109. uint32_t tmp = *p++;
  110. int16_t v1 = tmp & 0xFFFF;
  111. int16_t v2 = tmp >> 16;
  112. output[i] = sqrt_uint32_approx(v1 * v1 + v2 * v2);
  113. }
  114. }
  115. release(prevblocks[2]);
  116. prevblocks[2] = prevblocks[1];
  117. prevblocks[1] = prevblocks[0];
  118. prevblocks[0] = block;
  119. #endif
  120. }