diff --git a/filter_variable_f32.cpp b/filter_variable_f32.cpp new file mode 100644 index 0000000..21a94b3 --- /dev/null +++ b/filter_variable_f32.cpp @@ -0,0 +1,159 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * Copyright (c) 2018, Paul Geisler + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * 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 "filter_variable_f32.h" +#include +#include + +#if defined(KINETISK) + +__inline__ float clamp(float x, float m) +{ + return min(m,max(-m,x)); +} + +void AudioFilterStateVariable_F32::update_variable(const float *in, + const float *ctl, float *lp, float *bp, float *hp) +{ + const float *end = in + AUDIO_BLOCK_SAMPLES; + float input, inputprev; + float lowpass, bandpass, highpass; + float lowpasstmp, bandpasstmp, highpasstmp; + float fcenter, fmult, damp, octavemult; + + fcenter = setting_fcenter; + octavemult = setting_octavemult; + damp = setting_damp; + inputprev = state_inputprev; + lowpass = state_lowpass; + bandpass = state_bandpass; + int i=0; + // compute fmult using control input, fcenter and octavemult + // fmult is linearly interpolated across AUDIO_BLOCK_SAMPLES. + // as an exp2f call is not feasable for every single sample, + // it consumes too much CPU cycles on Teensy 3.5 to allow for multiple filters. + float fmult0=clamp(ctl ? exp2f(octavemult * ctl[0 ])*fcenter : fcenter, 0.8f); + float fmult1=clamp(ctl ? exp2f(octavemult * ctl[AUDIO_BLOCK_SAMPLES-1])*fcenter : fcenter, 0.8f); + do { + float t=((float)i++)/AUDIO_BLOCK_SAMPLES; + fmult=(1.f-t)*fmult0 + t*fmult1; + + input = *in++; + lowpass = lowpass + fmult * bandpass; + highpass = (input + inputprev)/2.f - lowpass - damp * bandpass; + inputprev = input; + bandpass = bandpass + (fmult * highpass); + lowpasstmp = lowpass; + bandpasstmp = bandpass; + highpasstmp = highpass; + lowpass = lowpass + fmult * bandpass; + highpass = input - lowpass - damp * bandpass; + bandpass = bandpass + fmult * highpass; + + lowpass= clamp(lowpass ,setting_limit); + bandpass=clamp(bandpass,setting_limit); + highpass=clamp(highpass,setting_limit); + + lowpasstmp = (lowpass+lowpasstmp) /2.f; + bandpasstmp = (bandpass+bandpasstmp)/2.f; + highpasstmp = (highpass+highpasstmp)/2.f; + + *lp++ = lowpasstmp; + *bp++ = bandpasstmp; + *hp++ = highpasstmp; + } while (in < end); + + state_inputprev = inputprev; + state_lowpass = lowpass; + state_bandpass = bandpass; +} + + +void AudioFilterStateVariable_F32::update(void) +{ + audio_block_f32_t *input_block=NULL, *control_block=NULL; + audio_block_f32_t *lowpass_block=NULL, *bandpass_block=NULL, *highpass_block=NULL; + + input_block = receiveReadOnly_f32(0); + control_block = receiveReadOnly_f32(1); + if (!input_block) { + if (control_block) AudioStream_F32::release(control_block); + return; + } + lowpass_block = allocate_f32(); + if (!lowpass_block) { + AudioStream_F32::release(input_block); + if (control_block) AudioStream_F32::release(control_block); + return; + } + bandpass_block = allocate_f32(); + if (!bandpass_block) { + AudioStream_F32::release(input_block); + AudioStream_F32::release(lowpass_block); + if (control_block) AudioStream_F32::release(control_block); + return; + } + highpass_block = allocate_f32(); + if (!highpass_block) { + AudioStream_F32::release(input_block); + AudioStream_F32::release(lowpass_block); + AudioStream_F32::release(bandpass_block); + if (control_block) release(control_block); + return; + } + + update_variable(input_block->data, + control_block ? control_block->data : NULL, + lowpass_block->data, + bandpass_block->data, + highpass_block->data); + AudioStream_F32::release(control_block); + + AudioStream_F32::release(input_block); + AudioStream_F32::transmit(lowpass_block, 0); + AudioStream_F32::release(lowpass_block); + AudioStream_F32::transmit(bandpass_block, 1); + AudioStream_F32::release(bandpass_block); + AudioStream_F32::transmit(highpass_block, 2); + AudioStream_F32::release(highpass_block); + return; +} + +#elif defined(KINETISL) + +void AudioFilterStateVariable_F32::update(void) +{ + audio_block_f32_t *block; + + block = receiveReadOnly_f32(0); + if (block) release(block); + block = receiveReadOnly_f32(1); + if (block) release(block); +} + +#endif + diff --git a/filter_variable_f32.h b/filter_variable_f32.h new file mode 100644 index 0000000..fc66401 --- /dev/null +++ b/filter_variable_f32.h @@ -0,0 +1,85 @@ +/* Audio Library for Teensy 3.X + * Copyright (c) 2014, Paul Stoffregen, paul@pjrc.com + * + * Development of this audio library was funded by PJRC.COM, LLC by sales of + * Teensy and Audio Adaptor boards. Please support PJRC's efforts to develop + * open source software by purchasing Teensy or other PJRC products. + * + * 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. + */ + +#ifndef filter_variable_f32_h_ +#define filter_variable_f32_h_ + +#include "Arduino.h" +#include "AudioStream_F32.h" + +class AudioFilterStateVariable_F32: public AudioStream_F32 +{ +public: + AudioFilterStateVariable_F32() : AudioStream_F32(2, inputQueueArray) { + frequency(1000); + octaveControl(1.0); // default values + resonance(0.707); + limit(5.f); + state_inputprev = 0; + state_lowpass = 0; + state_bandpass = 0; + } + void frequency(float freq) { + if (freq < 20.0) freq = 20.0; + else if (freq > AUDIO_SAMPLE_RATE_EXACT/2.5) freq = AUDIO_SAMPLE_RATE_EXACT/2.5; + setting_fcenter = (freq * (3.141592654/(AUDIO_SAMPLE_RATE_EXACT*2.0))); + } + void resonance(float q) { + if (q < 0.5) q = 0.5; + else if (q > 5.0) q = 5.0; + // TODO: allow lower Q when frequency is lower + setting_damp = (1.0 / q); + } + void octaveControl(float n) { + // filter's corner frequency is Fcenter * 2^(control * N) + // where "control" ranges from -1.0 to +1.0 + // and "N" allows the frequency to change from 0 to 7 octaves + if (n < 0.0) n = 0.0; + else if (n > 6.9999) n = 6.9999; + setting_octavemult = n; + } + + void limit(float l) + { + if(l<0.f) l=0.f; + setting_limit=l; + } + + virtual void update(void); +private: + void update_variable(const float *in, const float *ctl, + float *lp, float *bp, float *hp); + float setting_fcenter; + float setting_octavemult; + float setting_damp; + float setting_limit; + float state_inputprev; + float state_lowpass; + float state_bandpass; + audio_block_f32_t *inputQueueArray[2]; +}; + +#endif