visus
/
teensy-audio
Obserwuj 1
Polub 0
Forkuj 0
Kod Zgłoszenia 0 Oczekujące zmiany 0 Wydania 0 Wiki Aktywność
231 Commity
2 Gałęzie
Drzewo: 9f167ace1f
teensy-audio/filter_variable.cpp

224 lines
7.9KB
Czysty Wina Historia 

  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. 

  27. #include "filter_variable.h"

  28. #include "utility/dspinst.h"

  29. 

  30. // State Variable Filter (Chamberlin) with 2X oversampling

  31. // http://www.musicdsp.org/showArchiveComment.php?ArchiveID=92

  32. 

  33. // The fast 32x32 with rshift32 discards 2 bits, which probably

  34. // never matter.

  35. //#define MULT(a, b) (int32_t)(((int64_t)(a) * (b)) >> 30)

  36. #define MULT(a, b) (multiply_32x32_rshift32_rounded(a, b) << 2)

  37. 

  38. // It's very unlikely anyone could hear any difference, but if you

  39. // care deeply about numerical precision in seldom-used cases,

  40. // uncomment this to improve the control signal accuracy

  41. //#define IMPROVE_HIGH_FREQUENCY_ACCURACY

  42. 

  43. // This increases the exponential approximation accuracy from

  44. // about 0.341% error to only 0.012% error, which probably makes

  45. // no audible difference.

  46. //#define IMPROVE_EXPONENTIAL_ACCURACY

  47. 

  48. void AudioFilterStateVariable::update_fixed(const int16_t *in,

  49. 	int16_t *lp, int16_t *bp, int16_t *hp)

  50. {

  51. 	const int16_t *end = in + AUDIO_BLOCK_SAMPLES;

  52. 	int32_t input, inputprev;

  53. 	int32_t lowpass, bandpass, highpass;

  54. 	int32_t lowpasstmp, bandpasstmp, highpasstmp;

  55. 	int32_t fmult, damp;

  56. 

  57. 	fmult = setting_fmult;

  58. 	damp = setting_damp;

  59. 	inputprev = state_inputprev;

  60. 	lowpass = state_lowpass;

  61. 	bandpass = state_bandpass;

  62. 	do {

  63. 		input = (*in++) << 12;

  64. 		lowpass = lowpass + MULT(fmult, bandpass);

  65. 		highpass = ((input + inputprev)>>1) - lowpass - MULT(damp, bandpass);

  66. 		inputprev = input;

  67. 		bandpass = bandpass + MULT(fmult, highpass);

  68. 		lowpasstmp = lowpass;

  69. 		bandpasstmp = bandpass;

  70. 		highpasstmp = highpass;

  71. 		lowpass = lowpass + MULT(fmult, bandpass);

  72. 		highpass = input - lowpass - MULT(damp, bandpass);

  73. 		bandpass = bandpass + MULT(fmult, highpass);

  74. 		lowpasstmp = signed_saturate_rshift(lowpass+lowpasstmp, 16, 13);

  75. 		bandpasstmp = signed_saturate_rshift(bandpass+bandpasstmp, 16, 13);

  76. 		highpasstmp = signed_saturate_rshift(highpass+highpasstmp, 16, 13);

  77. 		*lp++ = lowpasstmp;

  78. 		*bp++ = bandpasstmp;

  79. 		*hp++ = highpasstmp;

  80. 	} while (in < end);

  81. 	state_inputprev = inputprev;

  82. 	state_lowpass = lowpass;

  83. 	state_bandpass = bandpass;

  84. }

  85. 

  86. 

  87. void AudioFilterStateVariable::update_variable(const int16_t *in,

  88. 	const int16_t *ctl, int16_t *lp, int16_t *bp, int16_t *hp)

  89. {

  90. 	const int16_t *end = in + AUDIO_BLOCK_SAMPLES;

  91. 	int32_t input, inputprev, control;

  92. 	int32_t lowpass, bandpass, highpass;

  93. 	int32_t lowpasstmp, bandpasstmp, highpasstmp;

  94. 	int32_t fcenter, fmult, damp, octavemult;

  95. 	int32_t n;

  96. 

  97. 	fcenter = setting_fcenter;

  98. 	octavemult = setting_octavemult;

  99. 	damp = setting_damp;

  100. 	inputprev = state_inputprev;

  101. 	lowpass = state_lowpass;

  102. 	bandpass = state_bandpass;

  103. 	do {

  104. 		// compute fmult using control input, fcenter and octavemult

  105. 		control = *ctl++;          // signal is always 15 fractional bits

  106. 		control *= octavemult;     // octavemult range: 0 to 28671 (12 frac bits)

  107. 		n = control & 0x7FFFFFF;   // 27 fractional control bits

  108. 		#ifdef IMPROVE_EXPONENTIAL_ACCURACY

  109. 		// exp2 polynomial suggested by Stefan Stenzel on "music-dsp"

  110. 		// mail list, Wed, 3 Sep 2014 10:08:55 +0200

  111. 		int32_t x = n << 3;

  112. 		n = multiply_accumulate_32x32_rshift32_rounded(536870912, x, 1494202713);

  113. 		int32_t sq = multiply_32x32_rshift32_rounded(x, x);

  114. 		n = multiply_accumulate_32x32_rshift32_rounded(n, sq, 1934101615);

  115. 		n = n + (multiply_32x32_rshift32_rounded(sq,

  116. 			multiply_32x32_rshift32_rounded(x, 1358044250)) << 1);

  117. 		n = n << 1;

  118. 		#else

  119. 		// exp2 algorithm by Laurent de Soras

  120. 		// http://www.musicdsp.org/showone.php?id=106

  121. 		n = (n + 134217728) << 3;

  122. 		n = multiply_32x32_rshift32_rounded(n, n);

  123. 		n = multiply_32x32_rshift32_rounded(n, 715827883) << 3;

  124. 		n = n + 715827882;

  125. 		#endif

  126. 		n = n >> (6 - (control >> 27)); // 4 integer control bits

  127. 		fmult = multiply_32x32_rshift32_rounded(fcenter, n);

  128. 		if (fmult > 5378279) fmult = 5378279;

  129. 		fmult = fmult << 8;

  130. 		// fmult is within 0.4% accuracy for all but the top 2 octaves

  131. 		// of the audio band.  This math improves accuracy above 5 kHz.

  132. 		// Without this, the filter still works fine for processing

  133. 		// high frequencies, but the filter's corner frequency response

  134. 		// can end up about 6% higher than requested.

  135. 		#ifdef IMPROVE_HIGH_FREQUENCY_ACCURACY

  136. 		// From "Fast Polynomial Approximations to Sine and Cosine"

  137. 		// Charles K Garrett, http://krisgarrett.net/

  138. 		fmult = (multiply_32x32_rshift32_rounded(fmult, 2145892402) +

  139. 			multiply_32x32_rshift32_rounded(

  140. 			multiply_32x32_rshift32_rounded(fmult, fmult),

  141. 			multiply_32x32_rshift32_rounded(fmult, -1383276101))) << 1;

  142. 		#endif

  143. 		// now do the state variable filter as normal, using fmult

  144. 		input = (*in++) << 12;

  145. 		lowpass = lowpass + MULT(fmult, bandpass);

  146. 		highpass = ((input + inputprev)>>1) - lowpass - MULT(damp, bandpass);

  147. 		inputprev = input;

  148. 		bandpass = bandpass + MULT(fmult, highpass);

  149. 		lowpasstmp = lowpass;

  150. 		bandpasstmp = bandpass;

  151. 		highpasstmp = highpass;

  152. 		lowpass = lowpass + MULT(fmult, bandpass);

  153. 		highpass = input - lowpass - MULT(damp, bandpass);

  154. 		bandpass = bandpass + MULT(fmult, highpass);

  155. 		lowpasstmp = signed_saturate_rshift(lowpass+lowpasstmp, 16, 13);

  156. 		bandpasstmp = signed_saturate_rshift(bandpass+bandpasstmp, 16, 13);

  157. 		highpasstmp = signed_saturate_rshift(highpass+highpasstmp, 16, 13);

  158. 		*lp++ = lowpasstmp;

  159. 		*bp++ = bandpasstmp;

  160. 		*hp++ = highpasstmp;

  161. 	} while (in < end);

  162. 	state_inputprev = inputprev;

  163. 	state_lowpass = lowpass;

  164. 	state_bandpass = bandpass;

  165. }

  166. 

  167. 

  168. void AudioFilterStateVariable::update(void)

  169. {

  170. 	audio_block_t *input_block=NULL, *control_block=NULL;

  171. 	audio_block_t *lowpass_block=NULL, *bandpass_block=NULL, *highpass_block=NULL;

  172. 

  173. 	input_block = receiveReadOnly(0);

  174. 	control_block = receiveReadOnly(1);

  175. 	if (!input_block) {

  176. 		if (control_block) release(control_block);

  177. 		return;

  178. 	}

  179. 	lowpass_block = allocate();

  180. 	if (!lowpass_block) {

  181. 		release(input_block);

  182. 		if (control_block) release(control_block);

  183. 		return;

  184. 	}

  185. 	bandpass_block = allocate();

  186. 	if (!bandpass_block) {

  187. 		release(input_block);

  188. 		release(lowpass_block);

  189. 		if (control_block) release(control_block);

  190. 		return;

  191. 	}

  192. 	highpass_block = allocate();

  193. 	if (!highpass_block) {

  194. 		release(input_block);

  195. 		release(lowpass_block);

  196. 		release(bandpass_block);

  197. 		if (control_block) release(control_block);

  198. 		return;

  199. 	}

  200. 

  201. 	if (control_block) {

  202. 		update_variable(input_block->data,

  203. 			 control_block->data,

  204. 			 lowpass_block->data,

  205. 			 bandpass_block->data,

  206. 			 highpass_block->data);

  207. 		release(control_block);

  208. 	} else {

  209. 		update_fixed(input_block->data,

  210. 			 lowpass_block->data,

  211. 			 bandpass_block->data,

  212. 			 highpass_block->data);

  213. 	}

  214. 	release(input_block);

  215. 	transmit(lowpass_block, 0);

  216. 	release(lowpass_block);

  217. 	transmit(bandpass_block, 1);

  218. 	release(bandpass_block);

  219. 	transmit(highpass_block, 2);

  220. 	release(highpass_block);

  221. 	return;

  222. }