This is a part of the Kinetiszer project.
filter.c@1:8ae4ab73ca6a, 2014-10-28 (annotated)
- Committer:
- Clemo
- Date:
- Tue Oct 28 20:09:12 2014 +0000
- Revision:
- 1:8ae4ab73ca6a
- Parent:
- 0:cb80470434eb
First publication (untested)
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Clemo | 0:cb80470434eb | 1 | /* |
Clemo | 0:cb80470434eb | 2 | Copyright 2013 Paul Soulsby www.soulsbysynths.com |
Clemo | 0:cb80470434eb | 3 | This file is part of Atmegatron. |
Clemo | 0:cb80470434eb | 4 | |
Clemo | 0:cb80470434eb | 5 | Atmegatron is free software: you can redistribute it and/or modify |
Clemo | 0:cb80470434eb | 6 | it under the terms of the GNU General Public License as published by |
Clemo | 0:cb80470434eb | 7 | the Free Software Foundation, either version 3 of the License, or |
Clemo | 0:cb80470434eb | 8 | (at your option) any later version. |
Clemo | 0:cb80470434eb | 9 | |
Clemo | 0:cb80470434eb | 10 | Atmegatron is distributed in the hope that it will be useful, |
Clemo | 0:cb80470434eb | 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
Clemo | 0:cb80470434eb | 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
Clemo | 0:cb80470434eb | 13 | GNU General Public License for more details. |
Clemo | 0:cb80470434eb | 14 | |
Clemo | 0:cb80470434eb | 15 | You should have received a copy of the GNU General Public License |
Clemo | 0:cb80470434eb | 16 | along with Atmegatron. If not, see <http://www.gnu.org/licenses/>. |
Clemo | 0:cb80470434eb | 17 | */ |
Clemo | 0:cb80470434eb | 18 | |
Clemo | 0:cb80470434eb | 19 | //***15 Biquad filter algorithms*** |
Clemo | 0:cb80470434eb | 20 | |
Clemo | 0:cb80470434eb | 21 | #include "atmegatron.h" |
Clemo | 0:cb80470434eb | 22 | |
Clemo | 0:cb80470434eb | 23 | const float pow_10_025 = 1.7782794100389228012254211951927; // pow(10,0.25) |
Clemo | 0:cb80470434eb | 24 | const float pow_10_075 = 5.6234132519034908039495103977648; // pow(10,0.75) |
Clemo | 0:cb80470434eb | 25 | const float pow_10_250 = 316.22776601683793319988935444327; // pow(10,2.5) |
Clemo | 0:cb80470434eb | 26 | |
Clemo | 0:cb80470434eb | 27 | //lets and gets |
Clemo | 0:cb80470434eb | 28 | byte filt_fc = 255; //filter cutoff - not the actual Fc, just a way to store knob position as byte (0-255) |
Clemo | 0:cb80470434eb | 29 | byte filt_q = 0; //filter resonance - not the actual Q, just a way to store knob position as byte (0-255) |
Clemo | 0:cb80470434eb | 30 | byte filt_type = 1; //filter type |
Clemo | 0:cb80470434eb | 31 | boolean filt_gainadj = false; //filter normalise mode (called gain adjust in code) |
Clemo | 0:cb80470434eb | 32 | byte filt_fenvamt = 0; //filter env amount |
Clemo | 0:cb80470434eb | 33 | byte filt_lfoamt = 0; //filter lfo amount |
Clemo | 0:cb80470434eb | 34 | |
Clemo | 0:cb80470434eb | 35 | //local vars |
Clemo | 0:cb80470434eb | 36 | 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 |
Clemo | 0:cb80470434eb | 37 | 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 |
Clemo | 0:cb80470434eb | 38 | float filt_fenvamtf = 0; //filter env amount as float (0-1) |
Clemo | 0:cb80470434eb | 39 | float filt_lfoamtf = 0; //filter lfo amount as float (0-1) |
Clemo | 0:cb80470434eb | 40 | float filt_fc_calc = 0; //final Fc for use in biquad equation (after mult with env, lfo etc) |
Clemo | 0:cb80470434eb | 41 | float filt_q_calc = 1; //final Q for use in biquad equation (after mult with env, lfo etc) |
Clemo | 0:cb80470434eb | 42 | float two_pi_over_sf= 0; //2pi / sample freq (ie interrupt freq) |
Clemo | 0:cb80470434eb | 43 | float pi_over_sf= 0; //pi / sample freq |
Clemo | 0:cb80470434eb | 44 | |
Clemo | 0:cb80470434eb | 45 | // filter constants - these are the constants used by biquad equation. Whenever Fc, Q or filter type changes, they need recalculating |
Clemo | 0:cb80470434eb | 46 | float a0=1; |
Clemo | 0:cb80470434eb | 47 | float a1; |
Clemo | 0:cb80470434eb | 48 | float a2; |
Clemo | 0:cb80470434eb | 49 | float b0; |
Clemo | 0:cb80470434eb | 50 | float b1; |
Clemo | 0:cb80470434eb | 51 | float b2; |
Clemo | 0:cb80470434eb | 52 | float A = 1.7782794100389228012254211951927; //pow(10, 0.25); |
Clemo | 0:cb80470434eb | 53 | |
Clemo | 0:cb80470434eb | 54 | |
Clemo | 0:cb80470434eb | 55 | // lets and gets |
Clemo | 0:cb80470434eb | 56 | //Filter cutoff frequency knob position (0-255) |
Clemo | 0:cb80470434eb | 57 | void Filt_Let_Fc(byte newfc) |
Clemo | 0:cb80470434eb | 58 | { |
Clemo | 0:cb80470434eb | 59 | filt_fc = newfc; |
Clemo | 0:cb80470434eb | 60 | } |
Clemo | 0:cb80470434eb | 61 | byte Filt_Get_Fc(void) |
Clemo | 0:cb80470434eb | 62 | { |
Clemo | 0:cb80470434eb | 63 | return filt_fc; |
Clemo | 0:cb80470434eb | 64 | } |
Clemo | 0:cb80470434eb | 65 | //Filter resonance (Q) knob position (0-255) |
Clemo | 0:cb80470434eb | 66 | void Filt_Let_Q(byte newq) |
Clemo | 0:cb80470434eb | 67 | { |
Clemo | 0:cb80470434eb | 68 | filt_q = newq; |
Clemo | 0:cb80470434eb | 69 | } |
Clemo | 0:cb80470434eb | 70 | byte Filt_Get_Q(void) |
Clemo | 0:cb80470434eb | 71 | { |
Clemo | 0:cb80470434eb | 72 | return filt_q; |
Clemo | 0:cb80470434eb | 73 | } |
Clemo | 0:cb80470434eb | 74 | //Filter type (0-15) |
Clemo | 0:cb80470434eb | 75 | void Filt_Let_Type(byte newtype){ |
Clemo | 0:cb80470434eb | 76 | if (newtype!=filt_type){ //set new filter type |
Clemo | 0:cb80470434eb | 77 | filt_type = newtype; |
Clemo | 0:cb80470434eb | 78 | switch (filt_type){ //calculate var A for shelf and peak filters: A = sqrt( 10^(dBgain/20) ) |
Clemo | 0:cb80470434eb | 79 | case FILT_PEAK10: |
Clemo | 0:cb80470434eb | 80 | //A = pow(10, 0.25); |
Clemo | 0:cb80470434eb | 81 | A = pow_10_025; |
Clemo | 0:cb80470434eb | 82 | break; |
Clemo | 0:cb80470434eb | 83 | case FILT_PEAK30: |
Clemo | 0:cb80470434eb | 84 | //A = pow(10, 0.75); |
Clemo | 0:cb80470434eb | 85 | A = pow_10_075; |
Clemo | 0:cb80470434eb | 86 | break; |
Clemo | 0:cb80470434eb | 87 | case FILT_PEAK100: |
Clemo | 0:cb80470434eb | 88 | //A = pow(10, 2.5); |
Clemo | 0:cb80470434eb | 89 | A = pow_10_250; |
Clemo | 0:cb80470434eb | 90 | break; |
Clemo | 0:cb80470434eb | 91 | case FILT_LS10: |
Clemo | 0:cb80470434eb | 92 | //A = pow(10, 0.25); |
Clemo | 0:cb80470434eb | 93 | A = pow_10_025; |
Clemo | 0:cb80470434eb | 94 | break; |
Clemo | 0:cb80470434eb | 95 | case FILT_LS30: |
Clemo | 0:cb80470434eb | 96 | //A = pow(10, 0.75); |
Clemo | 0:cb80470434eb | 97 | A = pow_10_075; |
Clemo | 0:cb80470434eb | 98 | break; |
Clemo | 0:cb80470434eb | 99 | case FILT_HS10: |
Clemo | 0:cb80470434eb | 100 | //A = pow(10, 0.25); |
Clemo | 0:cb80470434eb | 101 | A = pow_10_025; |
Clemo | 0:cb80470434eb | 102 | break; |
Clemo | 0:cb80470434eb | 103 | case FILT_HS30: |
Clemo | 0:cb80470434eb | 104 | //A = pow(10, 0.75); |
Clemo | 0:cb80470434eb | 105 | A = pow_10_075; |
Clemo | 0:cb80470434eb | 106 | break; |
Clemo | 0:cb80470434eb | 107 | } |
Clemo | 0:cb80470434eb | 108 | } |
Clemo | 0:cb80470434eb | 109 | } |
Clemo | 0:cb80470434eb | 110 | byte Filt_Get_Type(void) |
Clemo | 0:cb80470434eb | 111 | { |
Clemo | 0:cb80470434eb | 112 | return filt_type; |
Clemo | 0:cb80470434eb | 113 | } |
Clemo | 0:cb80470434eb | 114 | //set filter normalise mode (gainadj in code) |
Clemo | 0:cb80470434eb | 115 | void Filt_Let_GainAdj(boolean newadj){ |
Clemo | 0:cb80470434eb | 116 | filt_gainadj = newadj; |
Clemo | 0:cb80470434eb | 117 | } |
Clemo | 0:cb80470434eb | 118 | boolean Filt_Get_GainAdj(void) |
Clemo | 0:cb80470434eb | 119 | { |
Clemo | 0:cb80470434eb | 120 | return filt_gainadj; |
Clemo | 0:cb80470434eb | 121 | } |
Clemo | 0:cb80470434eb | 122 | |
Clemo | 0:cb80470434eb | 123 | // Filter meat |
Clemo | 0:cb80470434eb | 124 | void Filt_CalcVals(void) |
Clemo | 0:cb80470434eb | 125 | { |
Clemo | 0:cb80470434eb | 126 | unsigned long max_fc; //maximum cutoff frequency. Must be < Sf/2. Will ring at Sf/2. |
Clemo | 0:cb80470434eb | 127 | unsigned long f; //intermediate value for fict_fc_calc (before converting to float) |
Clemo | 0:cb80470434eb | 128 | |
Clemo | 0:cb80470434eb | 129 | if (filt_type>0){ //if filter is on |
Clemo | 0:cb80470434eb | 130 | max_fc = Master_Get_SampleFreq() >> 1UL; //calculate max cutoff frequency. Sf / 2 |
Clemo | 0:cb80470434eb | 131 | max_fc -= Master_Get_SampleFreq() >> 6UL; //minus a bit more for safety. reduce maxfc by proportion of Fs (about 1.5%) |
Clemo | 0:cb80470434eb | 132 | |
Clemo | 0:cb80470434eb | 133 | f = max_fc; //start with Fc at maximum |
Clemo | 0:cb80470434eb | 134 | f = f * (filt_fc + 1) >> 8UL; //then scale by filt cutoff knob position (use >>8 so knob=255 = max_fc) |
Clemo | 0:cb80470434eb | 135 | |
Clemo | 0:cb80470434eb | 136 | if (filt_lfoamt>0){ //if filt lfo knob > 0 |
Clemo | 0:cb80470434eb | 137 | 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) |
Clemo | 0:cb80470434eb | 138 | } |
Clemo | 0:cb80470434eb | 139 | |
Clemo | 0:cb80470434eb | 140 | if (filt_fenvamt>0){ |
Clemo | 0:cb80470434eb | 141 | 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) |
Clemo | 0:cb80470434eb | 142 | } |
Clemo | 0:cb80470434eb | 143 | if (f > max_fc){ //sort out if f > max_fc |
Clemo | 0:cb80470434eb | 144 | f = max_fc; |
Clemo | 0:cb80470434eb | 145 | } |
Clemo | 0:cb80470434eb | 146 | |
Clemo | 0:cb80470434eb | 147 | filt_fc_calc = (float)f; //convert to float ready for biquad calculation |
Clemo | 0:cb80470434eb | 148 | filt_q_calc = (float)filt_q * MULTQ + MINQ; //calculate q based on knob position. 0.5-20 |
Clemo | 0:cb80470434eb | 149 | pi_over_sf = PI/Master_Get_SampleFreq(); //used in biquad equations |
Clemo | 0:cb80470434eb | 150 | two_pi_over_sf = 2 * pi_over_sf; //used in biquad equations |
Clemo | 0:cb80470434eb | 151 | |
Clemo | 0:cb80470434eb | 152 | switch (filt_type){ //calulate biquad values |
Clemo | 0:cb80470434eb | 153 | case FILT_OFF: |
Clemo | 0:cb80470434eb | 154 | break; |
Clemo | 0:cb80470434eb | 155 | case FILT_LPF: |
Clemo | 0:cb80470434eb | 156 | LPValCalculator(); |
Clemo | 0:cb80470434eb | 157 | break; |
Clemo | 0:cb80470434eb | 158 | case FILT_HPF: |
Clemo | 0:cb80470434eb | 159 | HPValCalculator(); |
Clemo | 0:cb80470434eb | 160 | break; |
Clemo | 0:cb80470434eb | 161 | case FILT_BPF: |
Clemo | 0:cb80470434eb | 162 | BPSkirtValCalculator(); |
Clemo | 0:cb80470434eb | 163 | break; |
Clemo | 0:cb80470434eb | 164 | case FILT_NOTCH: |
Clemo | 0:cb80470434eb | 165 | NotchValCalculator(); |
Clemo | 0:cb80470434eb | 166 | break; |
Clemo | 0:cb80470434eb | 167 | case FILT_PEAK10: |
Clemo | 0:cb80470434eb | 168 | PeakingEQValCalculator(); |
Clemo | 0:cb80470434eb | 169 | break; |
Clemo | 0:cb80470434eb | 170 | case FILT_PEAK30: |
Clemo | 0:cb80470434eb | 171 | PeakingEQValCalculator(); |
Clemo | 0:cb80470434eb | 172 | break; |
Clemo | 0:cb80470434eb | 173 | case FILT_PEAK100: |
Clemo | 0:cb80470434eb | 174 | PeakingEQValCalculator(); |
Clemo | 0:cb80470434eb | 175 | break; |
Clemo | 0:cb80470434eb | 176 | case FILT_LS10: |
Clemo | 0:cb80470434eb | 177 | LowShelfValCalculator(); |
Clemo | 0:cb80470434eb | 178 | break; |
Clemo | 0:cb80470434eb | 179 | case FILT_LS30: |
Clemo | 0:cb80470434eb | 180 | LowShelfValCalculator(); |
Clemo | 0:cb80470434eb | 181 | break; |
Clemo | 0:cb80470434eb | 182 | case FILT_HS10: |
Clemo | 0:cb80470434eb | 183 | HighShelfValCalculator(); |
Clemo | 0:cb80470434eb | 184 | break; |
Clemo | 0:cb80470434eb | 185 | case FILT_HS30: |
Clemo | 0:cb80470434eb | 186 | HighShelfValCalculator(); |
Clemo | 0:cb80470434eb | 187 | break; |
Clemo | 0:cb80470434eb | 188 | case FILT_BUTLPF: |
Clemo | 0:cb80470434eb | 189 | ButterworthLPCalculator(); |
Clemo | 0:cb80470434eb | 190 | break; |
Clemo | 0:cb80470434eb | 191 | case FILT_BUTHPF: |
Clemo | 0:cb80470434eb | 192 | ButterworthHPCalculator(); |
Clemo | 0:cb80470434eb | 193 | break; |
Clemo | 0:cb80470434eb | 194 | case FILT_BESLPF: |
Clemo | 0:cb80470434eb | 195 | BesselLPCalculator(); |
Clemo | 0:cb80470434eb | 196 | break; |
Clemo | 0:cb80470434eb | 197 | case FILT_BESHPF: |
Clemo | 0:cb80470434eb | 198 | BesselHPCalculator(); |
Clemo | 0:cb80470434eb | 199 | break; |
Clemo | 0:cb80470434eb | 200 | } |
Clemo | 0:cb80470434eb | 201 | |
Clemo | 0:cb80470434eb | 202 | //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) |
Clemo | 0:cb80470434eb | 203 | |
Clemo | 0:cb80470434eb | 204 | b0 /= a0; |
Clemo | 0:cb80470434eb | 205 | b1 /= a0; |
Clemo | 0:cb80470434eb | 206 | b2 /= a0; |
Clemo | 0:cb80470434eb | 207 | a1 /= a0; |
Clemo | 0:cb80470434eb | 208 | a2 /= a0; |
Clemo | 0:cb80470434eb | 209 | |
Clemo | 0:cb80470434eb | 210 | } |
Clemo | 0:cb80470434eb | 211 | } |
Clemo | 0:cb80470434eb | 212 | |
Clemo | 0:cb80470434eb | 213 | // Calculate biquad filter constants for filter |
Clemo | 0:cb80470434eb | 214 | void LPValCalculator(void) |
Clemo | 0:cb80470434eb | 215 | { |
Clemo | 0:cb80470434eb | 216 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 217 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 218 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 219 | b0 = (1 - cs) / 2; |
Clemo | 0:cb80470434eb | 220 | b1 = 1 - cs; |
Clemo | 0:cb80470434eb | 221 | b2 = b0; |
Clemo | 0:cb80470434eb | 222 | a0 = 1 + alpha; |
Clemo | 0:cb80470434eb | 223 | a1 = -2 * cs; |
Clemo | 0:cb80470434eb | 224 | a2 = 1 - alpha; |
Clemo | 0:cb80470434eb | 225 | } |
Clemo | 0:cb80470434eb | 226 | |
Clemo | 0:cb80470434eb | 227 | void HPValCalculator(void) |
Clemo | 0:cb80470434eb | 228 | { |
Clemo | 0:cb80470434eb | 229 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 230 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 231 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 232 | b0 = (1 + cs) / 2; |
Clemo | 0:cb80470434eb | 233 | b1 = -(1 + cs); |
Clemo | 0:cb80470434eb | 234 | b2 = b0; |
Clemo | 0:cb80470434eb | 235 | a0 = 1 + alpha; |
Clemo | 0:cb80470434eb | 236 | a1 = -2 * cs; |
Clemo | 0:cb80470434eb | 237 | a2 = 1 - alpha; |
Clemo | 0:cb80470434eb | 238 | } |
Clemo | 0:cb80470434eb | 239 | |
Clemo | 0:cb80470434eb | 240 | void BPSkirtValCalculator(void) |
Clemo | 0:cb80470434eb | 241 | { |
Clemo | 0:cb80470434eb | 242 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 243 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 244 | float sn = sin(w); |
Clemo | 0:cb80470434eb | 245 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 246 | b0 = sn/2; |
Clemo | 0:cb80470434eb | 247 | b1 = 0; |
Clemo | 0:cb80470434eb | 248 | b2 = -b0; |
Clemo | 0:cb80470434eb | 249 | a0 = 1 + alpha; |
Clemo | 0:cb80470434eb | 250 | a1 = -2*cs; |
Clemo | 0:cb80470434eb | 251 | a2 = 1 - alpha; |
Clemo | 0:cb80470434eb | 252 | } |
Clemo | 0:cb80470434eb | 253 | void NotchValCalculator(void) |
Clemo | 0:cb80470434eb | 254 | { |
Clemo | 0:cb80470434eb | 255 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 256 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 257 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 258 | b0 = 1; |
Clemo | 0:cb80470434eb | 259 | b1 = -2*cs; |
Clemo | 0:cb80470434eb | 260 | b2 = 1; |
Clemo | 0:cb80470434eb | 261 | a0 = 1 + alpha; |
Clemo | 0:cb80470434eb | 262 | a1 = b1; |
Clemo | 0:cb80470434eb | 263 | a2 = 1 - alpha; |
Clemo | 0:cb80470434eb | 264 | } |
Clemo | 0:cb80470434eb | 265 | |
Clemo | 0:cb80470434eb | 266 | void PeakingEQValCalculator(void) |
Clemo | 0:cb80470434eb | 267 | { |
Clemo | 0:cb80470434eb | 268 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 269 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 270 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 271 | b0 = 1 + alpha*A; |
Clemo | 0:cb80470434eb | 272 | b1 = -2*cs; |
Clemo | 0:cb80470434eb | 273 | b2 = 1 - alpha*A; |
Clemo | 0:cb80470434eb | 274 | a0 = 1 + alpha/A; |
Clemo | 0:cb80470434eb | 275 | a1 = b1; |
Clemo | 0:cb80470434eb | 276 | a2 = 1 - alpha/A; |
Clemo | 0:cb80470434eb | 277 | } |
Clemo | 0:cb80470434eb | 278 | void LowShelfValCalculator(void) |
Clemo | 0:cb80470434eb | 279 | { |
Clemo | 0:cb80470434eb | 280 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 281 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 282 | float srA = sqrt(A); |
Clemo | 0:cb80470434eb | 283 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 284 | float amcs = (A-1)*cs; |
Clemo | 0:cb80470434eb | 285 | float apcs = (A+1)*cs; |
Clemo | 0:cb80470434eb | 286 | float sraaplpha = 2*srA*alpha; |
Clemo | 0:cb80470434eb | 287 | b0 = A*( (A+1) - amcs + sraaplpha ); |
Clemo | 0:cb80470434eb | 288 | b1 = 2*A*( (A-1) - apcs ); |
Clemo | 0:cb80470434eb | 289 | b2 = A*( (A+1) - amcs - sraaplpha ); |
Clemo | 0:cb80470434eb | 290 | a0 = (A+1) + amcs + sraaplpha ; |
Clemo | 0:cb80470434eb | 291 | a1 = -2*( (A-1) + apcs ); |
Clemo | 0:cb80470434eb | 292 | a2 = (A+1) + amcs - sraaplpha ; |
Clemo | 0:cb80470434eb | 293 | } |
Clemo | 0:cb80470434eb | 294 | void HighShelfValCalculator(void) |
Clemo | 0:cb80470434eb | 295 | { |
Clemo | 0:cb80470434eb | 296 | float w = two_pi_over_sf * filt_fc_calc; |
Clemo | 0:cb80470434eb | 297 | float cs = cos(w); |
Clemo | 0:cb80470434eb | 298 | float srA = sqrt(A); |
Clemo | 0:cb80470434eb | 299 | float alpha = sin(w) / filt_q_calc * 0.5; |
Clemo | 0:cb80470434eb | 300 | float amcs = (A-1)*cs; |
Clemo | 0:cb80470434eb | 301 | float apcs = (A+1)*cs; |
Clemo | 0:cb80470434eb | 302 | float sraaplpha = 2*srA*alpha; |
Clemo | 0:cb80470434eb | 303 | b0 = A*( (A+1) + amcs + sraaplpha ); |
Clemo | 0:cb80470434eb | 304 | b1 = -2*A*( (A-1) + apcs ); |
Clemo | 0:cb80470434eb | 305 | b2 = A*( (A+1) + amcs - sraaplpha ); |
Clemo | 0:cb80470434eb | 306 | a0 = (A+1) - amcs + sraaplpha ; |
Clemo | 0:cb80470434eb | 307 | a1 = 2*( (A-1) - apcs ); |
Clemo | 0:cb80470434eb | 308 | a2 = (A+1) - amcs - sraaplpha ; |
Clemo | 0:cb80470434eb | 309 | } |
Clemo | 0:cb80470434eb | 310 | void ButterworthLPCalculator(void) |
Clemo | 0:cb80470434eb | 311 | { |
Clemo | 0:cb80470434eb | 312 | float k = tan(filt_fc_calc * pi_over_sf); |
Clemo | 0:cb80470434eb | 313 | b2 = k * k; |
Clemo | 0:cb80470434eb | 314 | b0 = b2; |
Clemo | 0:cb80470434eb | 315 | b1 = 2 * b0; |
Clemo | 0:cb80470434eb | 316 | a0 = b0 + (SQRT2 * k) + 1; |
Clemo | 0:cb80470434eb | 317 | a1 = 2 * (b0 - 1); |
Clemo | 0:cb80470434eb | 318 | a2 = b0 - (SQRT2 * k) + 1; |
Clemo | 0:cb80470434eb | 319 | } |
Clemo | 0:cb80470434eb | 320 | void ButterworthHPCalculator(void) |
Clemo | 0:cb80470434eb | 321 | { |
Clemo | 0:cb80470434eb | 322 | float k = tan(filt_fc_calc * pi_over_sf); |
Clemo | 0:cb80470434eb | 323 | float k2p1 = k * k + 1; |
Clemo | 0:cb80470434eb | 324 | b0 = 1; |
Clemo | 0:cb80470434eb | 325 | b2 = b0; |
Clemo | 0:cb80470434eb | 326 | b1 = -2; |
Clemo | 0:cb80470434eb | 327 | a0 = k2p1 + (SQRT2 * k); |
Clemo | 0:cb80470434eb | 328 | a1 = 2 * (k2p1 - 2); |
Clemo | 0:cb80470434eb | 329 | a2 = k2p1 - (SQRT2 * k); |
Clemo | 0:cb80470434eb | 330 | } |
Clemo | 0:cb80470434eb | 331 | void BesselLPCalculator(void) |
Clemo | 0:cb80470434eb | 332 | { |
Clemo | 0:cb80470434eb | 333 | float w = tan(pi_over_sf * filt_fc_calc); |
Clemo | 0:cb80470434eb | 334 | b2 = 3 * w * w; |
Clemo | 0:cb80470434eb | 335 | b0 = b2; |
Clemo | 0:cb80470434eb | 336 | b1 = 2 * b0; |
Clemo | 0:cb80470434eb | 337 | a0 = 1 + 3 * w + b0; |
Clemo | 0:cb80470434eb | 338 | a1 = -2 + b1; |
Clemo | 0:cb80470434eb | 339 | a2 = 1 - 3 * w + b0; |
Clemo | 0:cb80470434eb | 340 | } |
Clemo | 0:cb80470434eb | 341 | void BesselHPCalculator(void) |
Clemo | 0:cb80470434eb | 342 | { |
Clemo | 0:cb80470434eb | 343 | float w = tan(pi_over_sf * filt_fc_calc); |
Clemo | 0:cb80470434eb | 344 | float w2 = w * w; |
Clemo | 0:cb80470434eb | 345 | b2 = 3; |
Clemo | 0:cb80470434eb | 346 | b0 = b2; |
Clemo | 0:cb80470434eb | 347 | b1 = -6; |
Clemo | 0:cb80470434eb | 348 | a0 = w2 + 3 * w + 3; |
Clemo | 0:cb80470434eb | 349 | a1 = 2 * w2 - 6; |
Clemo | 0:cb80470434eb | 350 | a2 = w2 - 3 * w + 3; |
Clemo | 0:cb80470434eb | 351 | } |
Clemo | 0:cb80470434eb | 352 | |
Clemo | 0:cb80470434eb | 353 | //Process the wavetable |
Clemo | 0:cb80470434eb | 354 | void Filt_Process(void) |
Clemo | 0:cb80470434eb | 355 | { |
Clemo | 0:cb80470434eb | 356 | byte i; |
Clemo | 0:cb80470434eb | 357 | sample_t out, maxout; |
Clemo | 0:cb80470434eb | 358 | float in, fout, multb, bout; |
Clemo | 0:cb80470434eb | 359 | |
Clemo | 0:cb80470434eb | 360 | if (filt_type>0){ |
Clemo | 0:cb80470434eb | 361 | maxout = 0; //used by filter normalise mode |
Clemo | 0:cb80470434eb | 362 | for (i=0;i<WAVE_LEN;i++){ //cycle through each sample |
Clemo | 0:cb80470434eb | 363 | if (filt_gainadj==true){ //if filter normalise mode on |
Clemo | 0:cb80470434eb | 364 | bout = Biquad_process((float)(Wave_Get_Process(i) >> 2)); //get sample and reduce amplitude. (default 2 = amp / 4), then perform biquad process |
Clemo | 0:cb80470434eb | 365 | } |
Clemo | 0:cb80470434eb | 366 | else { |
Clemo | 0:cb80470434eb | 367 | bout = Biquad_process((float)Wave_Get_Process(i)); //otherwise just get sample and perform biquad process |
Clemo | 0:cb80470434eb | 368 | } |
Clemo | 0:cb80470434eb | 369 | if (bout>127){ //constrain biquad output to max/min char value (by clipping) |
Clemo | 0:cb80470434eb | 370 | out = 127; |
Clemo | 0:cb80470434eb | 371 | } |
Clemo | 0:cb80470434eb | 372 | else if (bout<-128){ |
Clemo | 0:cb80470434eb | 373 | out = -128; |
Clemo | 0:cb80470434eb | 374 | } |
Clemo | 0:cb80470434eb | 375 | else{ |
Clemo | 0:cb80470434eb | 376 | out = (sample_t)bout; |
Clemo | 0:cb80470434eb | 377 | } |
Clemo | 0:cb80470434eb | 378 | Wave_Let_Process(i,out); //write sample back to wavetable |
Clemo | 0:cb80470434eb | 379 | if (filt_gainadj==true){ //if filter normalise mode on |
Clemo | 0:cb80470434eb | 380 | out = abs(out); //see if sample is greatest value in table and set maxout if it is |
Clemo | 0:cb80470434eb | 381 | if (out > maxout){ |
Clemo | 0:cb80470434eb | 382 | maxout = out; |
Clemo | 0:cb80470434eb | 383 | } |
Clemo | 0:cb80470434eb | 384 | } |
Clemo | 0:cb80470434eb | 385 | } |
Clemo | 0:cb80470434eb | 386 | if (filt_gainadj==true){ //if filter normalise mode |
Clemo | 0:cb80470434eb | 387 | multb = 128 / (float)maxout; //calculate multiplier to normalise waveform |
Clemo | 0:cb80470434eb | 388 | for (i=0;i<WAVE_LEN;i++){ //cycle through each sample |
Clemo | 0:cb80470434eb | 389 | in = (float)Wave_Get_Process(i); //get sample |
Clemo | 0:cb80470434eb | 390 | fout = in * multb; //multiply by normalise multiplier |
Clemo | 0:cb80470434eb | 391 | if (fout>127){ //clip sample if necessary |
Clemo | 0:cb80470434eb | 392 | out = 127; |
Clemo | 0:cb80470434eb | 393 | } |
Clemo | 0:cb80470434eb | 394 | else if (fout<-128){ |
Clemo | 0:cb80470434eb | 395 | out = -128; |
Clemo | 0:cb80470434eb | 396 | } |
Clemo | 0:cb80470434eb | 397 | else{ |
Clemo | 0:cb80470434eb | 398 | out = (sample_t)fout; |
Clemo | 0:cb80470434eb | 399 | } |
Clemo | 0:cb80470434eb | 400 | Wave_Let_Process(i,out); //write back again |
Clemo | 0:cb80470434eb | 401 | } |
Clemo | 0:cb80470434eb | 402 | } |
Clemo | 0:cb80470434eb | 403 | } |
Clemo | 0:cb80470434eb | 404 | } |
Clemo | 0:cb80470434eb | 405 | //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 |
Clemo | 0:cb80470434eb | 406 | float Biquad_process(float bi0) |
Clemo | 0:cb80470434eb | 407 | { |
Clemo | 0:cb80470434eb | 408 | static float bi1; |
Clemo | 0:cb80470434eb | 409 | static float bi2; |
Clemo | 0:cb80470434eb | 410 | float bo0; |
Clemo | 0:cb80470434eb | 411 | static float bo1; |
Clemo | 0:cb80470434eb | 412 | static float bo2; |
Clemo | 0:cb80470434eb | 413 | bo0 = b0 * bi0 + b1 * bi1 + b2 * bi2 - a1 * bo1 - a2 * bo2; |
Clemo | 0:cb80470434eb | 414 | bi2 = bi1; |
Clemo | 0:cb80470434eb | 415 | bi1 = bi0; |
Clemo | 0:cb80470434eb | 416 | bo2 = bo1; |
Clemo | 0:cb80470434eb | 417 | bo1 = bo0; |
Clemo | 0:cb80470434eb | 418 | return bo0; |
Clemo | 0:cb80470434eb | 419 | } |
Clemo | 0:cb80470434eb | 420 | |
Clemo | 0:cb80470434eb | 421 | //Filter LFO amount. This is stored as a byte representing knob position and a float (0-FILT_MAX) |
Clemo | 0:cb80470434eb | 422 | void Filt_Let_LFOAmt(byte newamt) |
Clemo | 0:cb80470434eb | 423 | { |
Clemo | 0:cb80470434eb | 424 | if (newamt!=filt_lfoamt){ //if new value different to current |
Clemo | 0:cb80470434eb | 425 | filt_lfoamt = newamt; //set new value |
Clemo | 0:cb80470434eb | 426 | filt_lfoamtf = (float)filt_lfoamt * FILT_MAX / 255; //calculate new float value used to calculate LFO lookup table (0-FILT_MAX) |
Clemo | 0:cb80470434eb | 427 | memset(filt_lfolookup,0,sizeof(filt_lfolookup)); //clear the lookup table (quicker than loop) |
Clemo | 0:cb80470434eb | 428 | } |
Clemo | 0:cb80470434eb | 429 | } |
Clemo | 0:cb80470434eb | 430 | byte Filt_Get_LFOAmt(void) |
Clemo | 0:cb80470434eb | 431 | { |
Clemo | 0:cb80470434eb | 432 | return filt_lfoamt; |
Clemo | 0:cb80470434eb | 433 | } |
Clemo | 0:cb80470434eb | 434 | //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 |
Clemo | 0:cb80470434eb | 435 | byte Filt_Get_LFOGain(void) |
Clemo | 0:cb80470434eb | 436 | { |
Clemo | 0:cb80470434eb | 437 | byte index; |
Clemo | 0:cb80470434eb | 438 | float f, g; |
Clemo | 0:cb80470434eb | 439 | index = LFO_Get_Level() + 127; //get the current output level of LFO (-127 - 128) and add offset (can't lookup negative array indexes) |
Clemo | 0:cb80470434eb | 440 | if (filt_lfolookup[index]==0){ //if index of lookup table hasn't yet been calculated: |
Clemo | 0:cb80470434eb | 441 | f = (float)LFO_Get_Level() / 127; //convert LFO output to float (0 - 1) |
Clemo | 0:cb80470434eb | 442 | 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 |
Clemo | 0:cb80470434eb | 443 | filt_lfolookup[index] = (byte)g; //write value to lookup table |
Clemo | 0:cb80470434eb | 444 | } |
Clemo | 0:cb80470434eb | 445 | return filt_lfolookup[index]; //return lookuptable value |
Clemo | 0:cb80470434eb | 446 | } |
Clemo | 0:cb80470434eb | 447 | //Filter Env amount. This is stored as a byte representing knob position and a float (0-FILT_MAX) |
Clemo | 0:cb80470434eb | 448 | void Filt_Let_FenvAmt(byte newamt) |
Clemo | 0:cb80470434eb | 449 | { |
Clemo | 0:cb80470434eb | 450 | if (newamt!=filt_fenvamt){ //if new value different to current |
Clemo | 0:cb80470434eb | 451 | filt_fenvamt = newamt; //set new value |
Clemo | 0:cb80470434eb | 452 | filt_fenvamtf = (float)filt_fenvamt * FILT_MAX / 255; //calculate new float value used to calculate env lookup table (0-FILT_MAX) |
Clemo | 0:cb80470434eb | 453 | memset(filt_fenvlookup,0,sizeof(filt_fenvlookup)); //clear the lookup table (quicker than loop) |
Clemo | 0:cb80470434eb | 454 | } |
Clemo | 0:cb80470434eb | 455 | } |
Clemo | 0:cb80470434eb | 456 | byte Filt_Get_FenvAmt(void) |
Clemo | 0:cb80470434eb | 457 | { |
Clemo | 0:cb80470434eb | 458 | return filt_fenvamt; |
Clemo | 0:cb80470434eb | 459 | } |
Clemo | 0:cb80470434eb | 460 | //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 |
Clemo | 0:cb80470434eb | 461 | byte Filt_Get_FenvGain(void) |
Clemo | 0:cb80470434eb | 462 | { |
Clemo | 0:cb80470434eb | 463 | byte index; |
Clemo | 0:cb80470434eb | 464 | float f, g; |
Clemo | 0:cb80470434eb | 465 | index = Fenv_Get_Level() + 127; //get the current output level of env (-127 - 128) and add offset (can't lookup negative array indexes) |
Clemo | 0:cb80470434eb | 466 | if (filt_fenvlookup[index]==0){ //if index of lookup table hasn't yet been calculated: |
Clemo | 0:cb80470434eb | 467 | f = (float)Fenv_Get_Level() / 127; //convert env output to float (0 - 1) |
Clemo | 0:cb80470434eb | 468 | 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 |
Clemo | 0:cb80470434eb | 469 | filt_fenvlookup[index] = (byte)g; //write value to lookup table |
Clemo | 0:cb80470434eb | 470 | } |
Clemo | 0:cb80470434eb | 471 | return filt_fenvlookup[index]; //return lookuptable value |
Clemo | 0:cb80470434eb | 472 | } |