This is a part of the Kinetiszer project.
Diff: filter.c
- Revision:
- 0:cb80470434eb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/filter.c Tue Oct 28 12:19:42 2014 +0000 @@ -0,0 +1,472 @@ +/* +Copyright 2013 Paul Soulsby www.soulsbysynths.com + This file is part of Atmegatron. + + Atmegatron is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + Atmegatron is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with Atmegatron. If not, see <http://www.gnu.org/licenses/>. +*/ + +//***15 Biquad filter algorithms*** + +#include "atmegatron.h" + +const float pow_10_025 = 1.7782794100389228012254211951927; // pow(10,0.25) +const float pow_10_075 = 5.6234132519034908039495103977648; // pow(10,0.75) +const float pow_10_250 = 316.22776601683793319988935444327; // pow(10,2.5) + +//lets and gets +byte filt_fc = 255; //filter cutoff - not the actual Fc, just a way to store knob position as byte (0-255) +byte filt_q = 0; //filter resonance - not the actual Q, just a way to store knob position as byte (0-255) +byte filt_type = 1; //filter type +boolean filt_gainadj = false; //filter normalise mode (called gain adjust in code) +byte filt_fenvamt = 0; //filter env amount +byte filt_lfoamt = 0; //filter lfo amount + +//local vars +byte filt_lfolookup[256]; //look up table to convert lfo output to Fc multiplier. Lookup saves having to do v slow exp calculation every cycle +byte filt_fenvlookup[256]; //look up table to convert env output to Fc multiplier. Lookup saves having to do v slow exp calculation every cycle +float filt_fenvamtf = 0; //filter env amount as float (0-1) +float filt_lfoamtf = 0; //filter lfo amount as float (0-1) +float filt_fc_calc = 0; //final Fc for use in biquad equation (after mult with env, lfo etc) +float filt_q_calc = 1; //final Q for use in biquad equation (after mult with env, lfo etc) +float two_pi_over_sf= 0; //2pi / sample freq (ie interrupt freq) +float pi_over_sf= 0; //pi / sample freq + +// filter constants - these are the constants used by biquad equation. Whenever Fc, Q or filter type changes, they need recalculating +float a0=1; +float a1; +float a2; +float b0; +float b1; +float b2; +float A = 1.7782794100389228012254211951927; //pow(10, 0.25); + + +// lets and gets +//Filter cutoff frequency knob position (0-255) +void Filt_Let_Fc(byte newfc) +{ + filt_fc = newfc; +} +byte Filt_Get_Fc(void) +{ + return filt_fc; +} +//Filter resonance (Q) knob position (0-255) +void Filt_Let_Q(byte newq) +{ + filt_q = newq; +} +byte Filt_Get_Q(void) +{ + return filt_q; +} +//Filter type (0-15) +void Filt_Let_Type(byte newtype){ + if (newtype!=filt_type){ //set new filter type + filt_type = newtype; + switch (filt_type){ //calculate var A for shelf and peak filters: A = sqrt( 10^(dBgain/20) ) + case FILT_PEAK10: + //A = pow(10, 0.25); + A = pow_10_025; + break; + case FILT_PEAK30: + //A = pow(10, 0.75); + A = pow_10_075; + break; + case FILT_PEAK100: + //A = pow(10, 2.5); + A = pow_10_250; + break; + case FILT_LS10: + //A = pow(10, 0.25); + A = pow_10_025; + break; + case FILT_LS30: + //A = pow(10, 0.75); + A = pow_10_075; + break; + case FILT_HS10: + //A = pow(10, 0.25); + A = pow_10_025; + break; + case FILT_HS30: + //A = pow(10, 0.75); + A = pow_10_075; + break; + } + } +} +byte Filt_Get_Type(void) +{ + return filt_type; +} +//set filter normalise mode (gainadj in code) +void Filt_Let_GainAdj(boolean newadj){ + filt_gainadj = newadj; +} +boolean Filt_Get_GainAdj(void) +{ + return filt_gainadj; +} + +// Filter meat +void Filt_CalcVals(void) +{ + unsigned long max_fc; //maximum cutoff frequency. Must be < Sf/2. Will ring at Sf/2. + unsigned long f; //intermediate value for fict_fc_calc (before converting to float) + + if (filt_type>0){ //if filter is on + max_fc = Master_Get_SampleFreq() >> 1UL; //calculate max cutoff frequency. Sf / 2 + max_fc -= Master_Get_SampleFreq() >> 6UL; //minus a bit more for safety. reduce maxfc by proportion of Fs (about 1.5%) + + f = max_fc; //start with Fc at maximum + f = f * (filt_fc + 1) >> 8UL; //then scale by filt cutoff knob position (use >>8 so knob=255 = max_fc) + + if (filt_lfoamt>0){ //if filt lfo knob > 0 + f = f * (Filt_Get_LFOGain() + 1) >> FILT_BS; //mult fc by lfo gain and bit shift. This is the fixed point maths way of mult fc by lfo (i.e. not using floating point) + } + + if (filt_fenvamt>0){ + f = f * (Filt_Get_FenvGain() + 1) >> FILT_BS; //mult fc by env gain and bit shift. This is the fixed point maths way of mult fc by env (i.e. not using floating point) + } + if (f > max_fc){ //sort out if f > max_fc + f = max_fc; + } + + filt_fc_calc = (float)f; //convert to float ready for biquad calculation + filt_q_calc = (float)filt_q * MULTQ + MINQ; //calculate q based on knob position. 0.5-20 + pi_over_sf = PI/Master_Get_SampleFreq(); //used in biquad equations + two_pi_over_sf = 2 * pi_over_sf; //used in biquad equations + + switch (filt_type){ //calulate biquad values + case FILT_OFF: + break; + case FILT_LPF: + LPValCalculator(); + break; + case FILT_HPF: + HPValCalculator(); + break; + case FILT_BPF: + BPSkirtValCalculator(); + break; + case FILT_NOTCH: + NotchValCalculator(); + break; + case FILT_PEAK10: + PeakingEQValCalculator(); + break; + case FILT_PEAK30: + PeakingEQValCalculator(); + break; + case FILT_PEAK100: + PeakingEQValCalculator(); + break; + case FILT_LS10: + LowShelfValCalculator(); + break; + case FILT_LS30: + LowShelfValCalculator(); + break; + case FILT_HS10: + HighShelfValCalculator(); + break; + case FILT_HS30: + HighShelfValCalculator(); + break; + case FILT_BUTLPF: + ButterworthLPCalculator(); + break; + case FILT_BUTHPF: + ButterworthHPCalculator(); + break; + case FILT_BESLPF: + BesselLPCalculator(); + break; + case FILT_BESHPF: + BesselHPCalculator(); + break; + } + + //This divide would normally be done in the Biquad_Process. However doing the time consuming divide here means that there's only 5 divides, rather than 32 (in Biquad_Process) + + b0 /= a0; + b1 /= a0; + b2 /= a0; + a1 /= a0; + a2 /= a0; + + } +} + +// Calculate biquad filter constants for filter +void LPValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float alpha = sin(w) / filt_q_calc * 0.5; + float cs = cos(w); + b0 = (1 - cs) / 2; + b1 = 1 - cs; + b2 = b0; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; +} + +void HPValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float alpha = sin(w) / filt_q_calc * 0.5; + float cs = cos(w); + b0 = (1 + cs) / 2; + b1 = -(1 + cs); + b2 = b0; + a0 = 1 + alpha; + a1 = -2 * cs; + a2 = 1 - alpha; +} + +void BPSkirtValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float alpha = sin(w) / filt_q_calc * 0.5; + float sn = sin(w); + float cs = cos(w); + b0 = sn/2; + b1 = 0; + b2 = -b0; + a0 = 1 + alpha; + a1 = -2*cs; + a2 = 1 - alpha; +} +void NotchValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float alpha = sin(w) / filt_q_calc * 0.5; + float cs = cos(w); + b0 = 1; + b1 = -2*cs; + b2 = 1; + a0 = 1 + alpha; + a1 = b1; + a2 = 1 - alpha; +} + +void PeakingEQValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float alpha = sin(w) / filt_q_calc * 0.5; + float cs = cos(w); + b0 = 1 + alpha*A; + b1 = -2*cs; + b2 = 1 - alpha*A; + a0 = 1 + alpha/A; + a1 = b1; + a2 = 1 - alpha/A; +} +void LowShelfValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float cs = cos(w); + float srA = sqrt(A); + float alpha = sin(w) / filt_q_calc * 0.5; + float amcs = (A-1)*cs; + float apcs = (A+1)*cs; + float sraaplpha = 2*srA*alpha; + b0 = A*( (A+1) - amcs + sraaplpha ); + b1 = 2*A*( (A-1) - apcs ); + b2 = A*( (A+1) - amcs - sraaplpha ); + a0 = (A+1) + amcs + sraaplpha ; + a1 = -2*( (A-1) + apcs ); + a2 = (A+1) + amcs - sraaplpha ; +} +void HighShelfValCalculator(void) +{ + float w = two_pi_over_sf * filt_fc_calc; + float cs = cos(w); + float srA = sqrt(A); + float alpha = sin(w) / filt_q_calc * 0.5; + float amcs = (A-1)*cs; + float apcs = (A+1)*cs; + float sraaplpha = 2*srA*alpha; + b0 = A*( (A+1) + amcs + sraaplpha ); + b1 = -2*A*( (A-1) + apcs ); + b2 = A*( (A+1) + amcs - sraaplpha ); + a0 = (A+1) - amcs + sraaplpha ; + a1 = 2*( (A-1) - apcs ); + a2 = (A+1) - amcs - sraaplpha ; +} +void ButterworthLPCalculator(void) +{ + float k = tan(filt_fc_calc * pi_over_sf); + b2 = k * k; + b0 = b2; + b1 = 2 * b0; + a0 = b0 + (SQRT2 * k) + 1; + a1 = 2 * (b0 - 1); + a2 = b0 - (SQRT2 * k) + 1; +} +void ButterworthHPCalculator(void) +{ + float k = tan(filt_fc_calc * pi_over_sf); + float k2p1 = k * k + 1; + b0 = 1; + b2 = b0; + b1 = -2; + a0 = k2p1 + (SQRT2 * k); + a1 = 2 * (k2p1 - 2); + a2 = k2p1 - (SQRT2 * k); +} +void BesselLPCalculator(void) +{ + float w = tan(pi_over_sf * filt_fc_calc); + b2 = 3 * w * w; + b0 = b2; + b1 = 2 * b0; + a0 = 1 + 3 * w + b0; + a1 = -2 + b1; + a2 = 1 - 3 * w + b0; +} +void BesselHPCalculator(void) +{ + float w = tan(pi_over_sf * filt_fc_calc); + float w2 = w * w; + b2 = 3; + b0 = b2; + b1 = -6; + a0 = w2 + 3 * w + 3; + a1 = 2 * w2 - 6; + a2 = w2 - 3 * w + 3; +} + +//Process the wavetable +void Filt_Process(void) +{ + byte i; + sample_t out, maxout; + float in, fout, multb, bout; + + if (filt_type>0){ + maxout = 0; //used by filter normalise mode + for (i=0;i<WAVE_LEN;i++){ //cycle through each sample + if (filt_gainadj==true){ //if filter normalise mode on + bout = Biquad_process((float)(Wave_Get_Process(i) >> 2)); //get sample and reduce amplitude. (default 2 = amp / 4), then perform biquad process + } + else { + bout = Biquad_process((float)Wave_Get_Process(i)); //otherwise just get sample and perform biquad process + } + if (bout>127){ //constrain biquad output to max/min char value (by clipping) + out = 127; + } + else if (bout<-128){ + out = -128; + } + else{ + out = (sample_t)bout; + } + Wave_Let_Process(i,out); //write sample back to wavetable + if (filt_gainadj==true){ //if filter normalise mode on + out = abs(out); //see if sample is greatest value in table and set maxout if it is + if (out > maxout){ + maxout = out; + } + } + } + if (filt_gainadj==true){ //if filter normalise mode + multb = 128 / (float)maxout; //calculate multiplier to normalise waveform + for (i=0;i<WAVE_LEN;i++){ //cycle through each sample + in = (float)Wave_Get_Process(i); //get sample + fout = in * multb; //multiply by normalise multiplier + if (fout>127){ //clip sample if necessary + out = 127; + } + else if (fout<-128){ + out = -128; + } + else{ + out = (sample_t)fout; + } + Wave_Let_Process(i,out); //write back again + } + } + } +} +//this is the standard biquad filter equation. normally would divide by a0, but this has already been done in filt_calcvals, to save the number of time consuming divisions required +float Biquad_process(float bi0) +{ + static float bi1; + static float bi2; + float bo0; + static float bo1; + static float bo2; + bo0 = b0 * bi0 + b1 * bi1 + b2 * bi2 - a1 * bo1 - a2 * bo2; + bi2 = bi1; + bi1 = bi0; + bo2 = bo1; + bo1 = bo0; + return bo0; +} + +//Filter LFO amount. This is stored as a byte representing knob position and a float (0-FILT_MAX) +void Filt_Let_LFOAmt(byte newamt) +{ + if (newamt!=filt_lfoamt){ //if new value different to current + filt_lfoamt = newamt; //set new value + filt_lfoamtf = (float)filt_lfoamt * FILT_MAX / 255; //calculate new float value used to calculate LFO lookup table (0-FILT_MAX) + memset(filt_lfolookup,0,sizeof(filt_lfolookup)); //clear the lookup table (quicker than loop) + } +} +byte Filt_Get_LFOAmt(void) +{ + return filt_lfoamt; +} +//Return the 'gain' of LFO. This is stored in a lookup table to save time consuming calculations each time. See the forums for further explanation of the 'gain' term +byte Filt_Get_LFOGain(void) +{ + byte index; + float f, g; + index = LFO_Get_Level() + 127; //get the current output level of LFO (-127 - 128) and add offset (can't lookup negative array indexes) + if (filt_lfolookup[index]==0){ //if index of lookup table hasn't yet been calculated: + f = (float)LFO_Get_Level() / 127; //convert LFO output to float (0 - 1) + g = round(exp(f * filt_lfoamtf) * FILT_MULT - 1); //calculate lookup table. e.g. lfoamt = 0: exp(0) * 64 - 1 = 63 e.g. lfoamt = 1 and lfo level = 127: exp(ln(4)) * 64 - 1 = 255 e.g. lfoamt = 1 and lfo = -127: exp(-ln(4)) * 64 - 1 = 15 + filt_lfolookup[index] = (byte)g; //write value to lookup table + } + return filt_lfolookup[index]; //return lookuptable value +} +//Filter Env amount. This is stored as a byte representing knob position and a float (0-FILT_MAX) +void Filt_Let_FenvAmt(byte newamt) +{ + if (newamt!=filt_fenvamt){ //if new value different to current + filt_fenvamt = newamt; //set new value + filt_fenvamtf = (float)filt_fenvamt * FILT_MAX / 255; //calculate new float value used to calculate env lookup table (0-FILT_MAX) + memset(filt_fenvlookup,0,sizeof(filt_fenvlookup)); //clear the lookup table (quicker than loop) + } +} +byte Filt_Get_FenvAmt(void) +{ + return filt_fenvamt; +} +//Return the 'gain' of env. This is stored in a lookup table to save time consuming calculations each time. See the forums for further explanation of the 'gain' term +byte Filt_Get_FenvGain(void) +{ + byte index; + float f, g; + index = Fenv_Get_Level() + 127; //get the current output level of env (-127 - 128) and add offset (can't lookup negative array indexes) + if (filt_fenvlookup[index]==0){ //if index of lookup table hasn't yet been calculated: + f = (float)Fenv_Get_Level() / 127; //convert env output to float (0 - 1) + g = round(exp(f * filt_fenvamtf) * FILT_MULT - 1); //calculate lookup table. e.g. envamt = 0: exp(0) * 64 - 1 = 63 e.g. envamt = 1 and lfo level = 127: exp(ln(4)) * 64 - 1 = 255 e.g. envamt = 1 and env = -127: exp(-ln(4)) * 64 - 1 = 15 + filt_fenvlookup[index] = (byte)g; //write value to lookup table + } + return filt_fenvlookup[index]; //return lookuptable value +}