ParametricEQ.cpp

00001 /**
00002  * @file    ParametricEQ.cpp
00003  * @brief   ParametricEQ - fixed point implementation of a Parametric EQ
00004  * @author  Patrick Thomas
00005  * @version 1.0
00006  * @see
00007  *
00008  * Copyright (c) 2016
00009  *
00010  * Licensed under the Apache License, Version 2.0 (the "License");
00011  * you may not use this file except in compliance with the License.
00012  * You may obtain a copy of the License at
00013  *
00014  *     http://www.apache.org/licenses/LICENSE-2.0
00015  *
00016  * Unless required by applicable law or agreed to in writing, software
00017  * distributed under the License is distributed on an "AS IS" BASIS,
00018  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00019  * See the License for the specific language governing permissions and
00020  * limitations under the License.
00021  */
00022 
00023 #include "ParametricEQ.h"
00024 
00025 int ParametricEQ::biquad::execute(int input) {
00026 
00027     int block_output = bhat_0*input + v1;
00028     w1 = bhat_1*input - ahat_1*(block_output/scaling_factor) + v3;
00029     v1 = c0*(w1/scaling_factor) - s0*(v2/scaling_factor);
00030     v2 = s0*(w1/scaling_factor) + c0*(v2/scaling_factor);
00031     w2 = bhat_2*input - ahat_2*(block_output/scaling_factor);
00032     v3 = c0*(w2/scaling_factor) - s0*(v4/scaling_factor);
00033     v4 = s0*(w2/scaling_factor) + c0*(v4/scaling_factor);
00034 
00035     return PIN(block_output/scaling_factor, -sample_bounds, sample_bounds);
00036 }
00037 
00038 void ParametricEQ::check_GBW() {
00039     
00040     // Find upper and lower bounds
00041     float upper = Gain_amplitude > G0_amplitude ? Gain_amplitude : G0_amplitude;
00042     float lower = Gain_amplitude < G0_amplitude ? G0_amplitude : Gain_amplitude;
00043     
00044     // Convert current GBW_dB into amplitude form
00045     Bandwidth_gain_amplitude = pow(10, GBW_dB/20);
00046     
00047     // Check this against the limits and clip if necessary
00048     Bandwidth_gain_amplitude = PIN(Bandwidth_gain_amplitude, lower + GBW_MARGIN, upper - GBW_MARGIN);
00049     
00050     // Convert the checked value back to decibel form
00051     GBW_dB = 20*log10(Bandwidth_gain_amplitude);
00052 } 
00053 
00054 void ParametricEQ::calculate() {
00055     
00056     // Calculate sampling variables
00057     Scaling_factor = 1 << (30 - Sample_bits);
00058     Sample_bounds = (1 << (Sample_bits - 1)) - 1;
00059 
00060     // Calculate parameter variables
00061     Normalised_centre_frequency = (F0_Hz*2*M_PI)/Sample_rate;
00062     Normalised_bandwidth = (BW_Hz*2*M_PI)/Sample_rate;
00063     f0_cosine = cos(Normalised_centre_frequency);
00064     f0_sine = sin(Normalised_centre_frequency);
00065     g = pow(Gain_amplitude,double(1)/Order);
00066     g_squared = g*g;
00067     g0 = pow(G0_amplitude,double(1)/Order);
00068     g0_squared = g0*g0;
00069     epsilon = sqrt((Gain_amplitude*Gain_amplitude - Bandwidth_gain_amplitude*Bandwidth_gain_amplitude)/
00070                    (Bandwidth_gain_amplitude*Bandwidth_gain_amplitude - G0_amplitude*G0_amplitude));
00071     beta = tan(Normalised_bandwidth/2)/pow(epsilon,double(1)/Order);
00072     beta_squared = beta*beta;
00073     counter = (Order + 1)/2;
00074     
00075     update_blocks();
00076 }
00077 
00078 void ParametricEQ::update_blocks() {
00079 
00080     // Iterate through filter blocks
00081     for (int i = 0; i < counter; i++)
00082     {
00083         // Assign new sampling parameters
00084         fx_blocks[i].scaling_factor = Scaling_factor;
00085         fx_blocks[i].sample_bounds = Sample_bounds;
00086 
00087         // Assign new coefficients
00088         fx_blocks[i].c0 = f0_cosine*Scaling_factor;
00089         fx_blocks[i].s0 = f0_sine*Scaling_factor;
00090 
00091         if ((Order%2 == 1) && (i == 0))
00092         {
00093             D = beta + 1;
00094             fx_blocks[i].bhat_0 = ((g*beta + g0)/D)*Scaling_factor;
00095             fx_blocks[i].bhat_1 = ((g*beta - g0)/D)*Scaling_factor;
00096             fx_blocks[i].bhat_2 = 0;
00097             fx_blocks[i].ahat_1 = ((beta - 1)/D)*Scaling_factor;
00098             fx_blocks[i].ahat_2 = 0;
00099         }
00100 
00101         else
00102         {
00103             phi = (((2*i) + 1)*M_PI)/(2*Order);
00104             si = sin(double(phi));
00105             D = beta_squared + 2*si*beta + 1;
00106             fx_blocks[i].bhat_0 = ((g_squared*beta_squared + 2*g*g0*si*beta + g0_squared)/D)*Scaling_factor;
00107             fx_blocks[i].bhat_1 = (2*(g_squared*beta_squared - g0_squared)/D)*Scaling_factor;
00108             fx_blocks[i].bhat_2 = ((g_squared*beta_squared - 2*g*g0*si*beta + g0_squared)/D)*Scaling_factor;
00109             fx_blocks[i].ahat_1 = (2*(beta_squared - 1)/D)*Scaling_factor;
00110             fx_blocks[i].ahat_2 = ((beta_squared - 2*si*beta + 1)/D)*Scaling_factor;
00111         }
00112     }
00113 }
00114 
00115 ParametricEQ::ParametricEQ() {
00116 
00117     // Initial parameter values
00118     Gain_dB = 6;
00119     GBW_dB = 3;
00120     F0_Hz = 4000;
00121     BW_Hz = 1000;
00122     Order = 2;
00123     Type = Peaking;
00124     
00125     // Initial sampling values
00126     Sample_rate = 48000;
00127     Sample_bits = 16;
00128 
00129     // Initialise filter blocks
00130     calculate();
00131 }
00132 
00133 float ParametricEQ::set_Gain_dB(float value) {
00134     
00135     Gain_dB = PIN(value, -GAIN_DB_MAX, GAIN_DB_MAX);
00136     
00137     set_Type(Type);
00138     return Gain_dB;
00139 }
00140 
00141 float ParametricEQ::set_GBW_dB(float value) {
00142     
00143     GBW_dB = PIN(value, -GAIN_DB_MAX, GAIN_DB_MAX);
00144     
00145     set_Type(Type);
00146     return GBW_dB;
00147 }
00148 
00149 int ParametricEQ::set_F0_Hz(int value) {
00150     
00151     F0_Hz = PIN(value, 0, Sample_rate/2);
00152     
00153     set_Type(Type);
00154     return F0_Hz;    
00155 }
00156 
00157 int ParametricEQ::set_BW_Hz(int value) {
00158         
00159     BW_Hz = PIN(value, BW_HZ_MIN, Sample_rate/2);
00160     
00161     set_Type(Type);
00162     return BW_Hz; 
00163 }
00164 
00165 int ParametricEQ::set_Order(int value) {
00166     
00167     Order = PIN(value, 1, MAX_ORDER);
00168     
00169     set_Type(Type);
00170     return Order; 
00171 }
00172 
00173 FilterType ParametricEQ::set_Type(FilterType type) {
00174     
00175     Type = type;
00176     
00177     // Tailor parameters to suit the chosen filter type
00178     switch (Type) {
00179         
00180         case Peaking:
00181             Gain_amplitude = pow(10, Gain_dB/20);
00182             G0_amplitude = 1;
00183             break;
00184         
00185         case BandPass:
00186             Gain_amplitude = 1;
00187             G0_amplitude = 0;
00188             break;
00189             
00190         case BandStop:
00191             Gain_amplitude = 0;
00192             G0_amplitude = 1;
00193             break;    
00194         
00195         case LowShelf:
00196             Gain_amplitude = pow(10, Gain_dB/20);
00197             G0_amplitude = 1;
00198             F0_Hz = 0;
00199             break;
00200         
00201         case HighShelf:
00202             Gain_amplitude = pow(10, Gain_dB/20);
00203             G0_amplitude = 1;
00204             F0_Hz = Sample_rate/2;
00205             break;
00206         
00207         case LowPass:
00208             Gain_amplitude = 1;
00209             G0_amplitude = 0;
00210             F0_Hz = 0;
00211             break;
00212             
00213         case HighPass:
00214             Gain_amplitude = 1;
00215             G0_amplitude = 0;
00216             F0_Hz = Sample_rate/2;
00217             break;
00218     }
00219     
00220     check_GBW();
00221     calculate();
00222     return Type;
00223 }
00224 
00225 int ParametricEQ::set_Sample_rate(int value) {
00226     
00227     Sample_rate = PIN(value, SAMPLE_RATE_MIN, SAMPLE_RATE_MAX);
00228     
00229     set_Type(Type);
00230     return Sample_rate; 
00231 }
00232 
00233 int ParametricEQ::set_Sample_bits(int value) {
00234     
00235     Sample_bits = PIN(value, SAMPLE_BITS_MIN, SAMPLE_BITS_MAX);
00236     
00237     set_Type(Type);
00238     return Sample_bits; 
00239 }
00240 
00241 int ParametricEQ::filter(int input) {
00242     
00243     // Send sample through filter blocks
00244     for (int i = 0; i < counter; i++)
00245     {
00246         input = fx_blocks[i].execute(input);        
00247     }
00248     
00249     return input;
00250 }
00251