204 rindas
6.3KB

  1. /* Audio Library for Teensy 3.X
  2. * Copyright (c) 2019, 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. /*
  27. by Alexander Walch
  28. */
  29. #ifndef biquad_coeffs_h_
  30. #define biquad_coeffs_h_
  31. #include "Arduino.h"
  32. #include <arm_math.h>
  33. enum class BiquadType {
  34. LOW_PASS, HIGH_PASS, BAND_PASS, NOTCH, ALL_PASS, PEAKING, LOW_SHELF, HIGH_SHELF
  35. };
  36. template <typename T>
  37. void getCoefficients(T* coeffs, BiquadType type, double dbGain, double freq, double srate, double bandwidthOrQOrS, bool isBandwidthOrS=false){
  38. const double A =(type == BiquadType::PEAKING || type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) ? pow(10., dbGain / 40.) : pow(10, dbGain / 20);
  39. const double omega = 2 * M_PI * freq / srate;
  40. const double sn = sin(omega);
  41. const double cs = cos(omega);
  42. double alpha;
  43. if (!isBandwidthOrS) // Q
  44. alpha = sn / (2 * bandwidthOrQOrS);
  45. else if (type == BiquadType::LOW_SHELF || type == BiquadType::HIGH_SHELF) // S
  46. alpha = sn / 2 * sqrt((A + 1 / A) * (1 / bandwidthOrQOrS - 1) + 2);
  47. else // BW
  48. alpha = sn * sinh(_M_LN2 / 2 * bandwidthOrQOrS * omega / sn);
  49. const double beta = 2 * sqrt(A) * alpha;
  50. double b0, b1, b2, a0Inv, a1, a2;
  51. switch (type)
  52. {
  53. case BiquadType::LOW_PASS:
  54. b0 = (1 - cs) / 2;
  55. b1 = 1 - cs;
  56. b2 = (1 - cs) / 2;
  57. a0Inv = 1/(1 + alpha);
  58. a1 = -2 * cs;
  59. a2 = 1 - alpha;
  60. break;
  61. case BiquadType::HIGH_PASS:
  62. b0 = (1 + cs) / 2;
  63. b1 = -(1 + cs);
  64. b2 = (1 + cs) / 2;
  65. a0Inv = 1/(1 + alpha);
  66. a1 = -2 * cs;
  67. a2 = 1 - alpha;
  68. break;
  69. case BiquadType::BAND_PASS:
  70. b0 = alpha;
  71. b1 = 0;
  72. b2 = -alpha;
  73. a0Inv = 1/(1 + alpha);
  74. a1 = -2 * cs;
  75. a2 = 1 - alpha;
  76. break;
  77. case BiquadType::NOTCH:
  78. b0 = 1;
  79. b1 = -2 * cs;
  80. b2 = 1;
  81. a0Inv = 1/(1 + alpha);
  82. a1 = -2 * cs;
  83. a2 = 1 - alpha;
  84. break;
  85. case BiquadType::ALL_PASS:
  86. b0 = 1 - alpha;
  87. b1 = -2 * cs;
  88. b2 = 1 + alpha;
  89. a0Inv = 1/(1 + alpha);
  90. a1 = -2 * cs;
  91. a2 = 1 - alpha;
  92. break;
  93. case BiquadType::PEAKING:
  94. b0 = 1 + (alpha * A);
  95. b1 = -2 * cs;
  96. b2 = 1 - (alpha * A);
  97. a0Inv = 1/(1 + (alpha / A));
  98. a1 = -2 * cs;
  99. a2 = 1 - (alpha / A);
  100. break;
  101. case BiquadType::LOW_SHELF:
  102. b0 = A * ((A + 1) - (A - 1) * cs + beta);
  103. b1 = 2 * A * ((A - 1) - (A + 1) * cs);
  104. b2 = A * ((A + 1) - (A - 1) * cs - beta);
  105. a0Inv = (A + 1) + (A - 1) * cs + beta;
  106. a1 = -2 * ((A - 1) + (A + 1) * cs);
  107. a2 = (A + 1) + (A - 1) * cs - beta;
  108. break;
  109. case BiquadType::HIGH_SHELF:
  110. b0 = A * ((A + 1) + (A - 1) * cs + beta);
  111. b1 = -2 * A * ((A - 1) + (A + 1) * cs);
  112. b2 = A * ((A + 1) + (A - 1) * cs - beta);
  113. a0Inv = 1/((A + 1) - (A - 1) * cs + beta);
  114. a1 = 2 * ((A - 1) - (A + 1) * cs);
  115. a2 = (A + 1) - (A - 1) * cs - beta;
  116. break;
  117. }
  118. *coeffs++=(T)(b0 * a0Inv);
  119. *coeffs++=(T)(b1 * a0Inv);
  120. *coeffs++=(T)(b2 * a0Inv);
  121. *coeffs++=(T)(-a1 * a0Inv);
  122. *coeffs=(T)(-a2 * a0Inv);
  123. }
  124. template <typename T, typename BIQUAD, typename BTYPE>
  125. void biquad_cascade_df2T(const BIQUAD* S, T* pSrc, T* pDst, uint32_t blockSize) {
  126. BTYPE* b0 =S->pCoeffs;
  127. BTYPE* b1=S->pCoeffs+1;
  128. BTYPE* b2=S->pCoeffs+2;
  129. BTYPE* a1Neg=S->pCoeffs+3;
  130. BTYPE* a2Neg=S->pCoeffs+4;
  131. BTYPE* state=S->pState;
  132. if(S->numStages==1){
  133. BTYPE yn;
  134. for (uint32_t j=0; j<blockSize; j++ ){
  135. yn = *b0 * *pSrc + *state;
  136. *state = *b1 * *pSrc + *a1Neg * yn + *(state+1);
  137. *(state+1) = *b2 * *pSrc++ + *a2Neg * yn;
  138. *pDst++=(T)yn;
  139. }
  140. }
  141. else {
  142. BTYPE pDstD[blockSize];
  143. BTYPE* pDstDP=pDstD;
  144. for (uint32_t j=0; j<blockSize; j++ ){
  145. *pDstDP = *b0 * *pSrc + *state;
  146. *state = *b1 * *pSrc + *a1Neg * *pDstDP + *(state+1);
  147. *(state+1) = *b2 * *pSrc++ + *a2Neg * *pDstDP++;
  148. }
  149. b0+=5;
  150. b1+=5;
  151. b2+=5;
  152. a1Neg+=5;
  153. a2Neg+=5;
  154. state+=2;
  155. for (uint8_t i =0; i< S->numStages - 2;i++){
  156. pDstDP=pDstD;
  157. BTYPE xn;
  158. for (uint32_t j=0; j<blockSize; j++ ){
  159. xn=*pDstDP;
  160. *pDstDP = *b0 * xn + *state;
  161. *state = *b1 * xn + *a1Neg * *pDstDP + *(state+1);
  162. *(state+1) = *b2 * xn + *a2Neg * *pDstDP++;
  163. }
  164. b0+=5;
  165. b1+=5;
  166. b2+=5;
  167. a1Neg+=5;
  168. a2Neg+=5;
  169. state+=2;
  170. }
  171. BTYPE yn;
  172. pDstDP=pDstD;
  173. for (uint32_t j=0; j<blockSize; j++ ){
  174. yn = *b0 * *pDstDP + *state;
  175. *state = *b1 * *pDstDP + *a1Neg * yn + *(state+1);
  176. *(state+1) = *b2 * *pDstDP++ + *a2Neg * yn;
  177. *pDst++=(T)yn;
  178. }
  179. }
  180. }
  181. template <typename B>
  182. void preload(const B* S, double val=0.){
  183. // double* b1=B->pCoeffs+1;
  184. // double* b2=B->pCoeffs+2;
  185. // double* a1Neg=B->pCoeffs+3;
  186. // double* a2Neg=B->pCoeffs+4;
  187. *(S->pState+1) = (*(S->pCoeffs+2) + *(S->pCoeffs+4)) * val;
  188. *(S->pState) = (*(S->pCoeffs+1) + *(S->pCoeffs+3)) * val + *(S->pState+1);
  189. }
  190. #endif