A parametric EQ that uses a fixed point, direct form 2 transposed, implementation of a Butterworth filter. The following parameters can be adjusted: Gain (dB), Bandwidth Gain (dB), Centre frequency (Hz), Bandwidth (Hz), Order, Type (Peaking, Bandstop, Bandpass, Low Shelf, High Shelf, Low Pass or High Pass).

Committer:
Padman
Date:
Thu Aug 04 19:34:39 2016 +0000
Revision:
0:2e17a3d6907c
Child:
1:0545d8890dc6
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Padman 0:2e17a3d6907c 1 /**
Padman 0:2e17a3d6907c 2 * @file ParametricEQ.cpp
Padman 0:2e17a3d6907c 3 * @brief ParametricEQ - fixed point implementation of a Parametric EQ
Padman 0:2e17a3d6907c 4 * @author Patrick Thomas
Padman 0:2e17a3d6907c 5 * @version 1.0
Padman 0:2e17a3d6907c 6 * @see
Padman 0:2e17a3d6907c 7 *
Padman 0:2e17a3d6907c 8 * Copyright (c) 2016
Padman 0:2e17a3d6907c 9 *
Padman 0:2e17a3d6907c 10 * Licensed under the Apache License, Version 2.0 (the "License");
Padman 0:2e17a3d6907c 11 * you may not use this file except in compliance with the License.
Padman 0:2e17a3d6907c 12 * You may obtain a copy of the License at
Padman 0:2e17a3d6907c 13 *
Padman 0:2e17a3d6907c 14 * http://www.apache.org/licenses/LICENSE-2.0
Padman 0:2e17a3d6907c 15 *
Padman 0:2e17a3d6907c 16 * Unless required by applicable law or agreed to in writing, software
Padman 0:2e17a3d6907c 17 * distributed under the License is distributed on an "AS IS" BASIS,
Padman 0:2e17a3d6907c 18 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
Padman 0:2e17a3d6907c 19 * See the License for the specific language governing permissions and
Padman 0:2e17a3d6907c 20 * limitations under the License.
Padman 0:2e17a3d6907c 21 */
Padman 0:2e17a3d6907c 22
Padman 0:2e17a3d6907c 23 #include "ParametricEQ.h"
Padman 0:2e17a3d6907c 24
Padman 0:2e17a3d6907c 25 int biquad::execute(int input) {
Padman 0:2e17a3d6907c 26
Padman 0:2e17a3d6907c 27 int block_output = bhat_0*input + v1;
Padman 0:2e17a3d6907c 28 w1 = bhat_1*input - ahat_1*(block_output/scaling_factor) + v3;
Padman 0:2e17a3d6907c 29 v1 = c0*(w1/scaling_factor) - s0*(v2/scaling_factor);
Padman 0:2e17a3d6907c 30 v2 = s0*(w1/scaling_factor) + c0*(v2/scaling_factor);
Padman 0:2e17a3d6907c 31 w2 = bhat_2*input - ahat_2*(block_output/scaling_factor);
Padman 0:2e17a3d6907c 32 v3 = c0*(w2/scaling_factor) - s0*(v4/scaling_factor);
Padman 0:2e17a3d6907c 33 v4 = s0*(w2/scaling_factor) + c0*(v4/scaling_factor);
Padman 0:2e17a3d6907c 34
Padman 0:2e17a3d6907c 35 return PIN(block_output/scaling_factor, -sample_bounds, sample_bounds);
Padman 0:2e17a3d6907c 36 }
Padman 0:2e17a3d6907c 37
Padman 0:2e17a3d6907c 38 void ParametricEQ::check_GBW() {
Padman 0:2e17a3d6907c 39
Padman 0:2e17a3d6907c 40 // Find upper and lower bounds
Padman 0:2e17a3d6907c 41 float upper = Gain_amplitude > G0_amplitude ? Gain_amplitude : G0_amplitude;
Padman 0:2e17a3d6907c 42 float lower = Gain_amplitude < G0_amplitude ? G0_amplitude : Gain_amplitude;
Padman 0:2e17a3d6907c 43
Padman 0:2e17a3d6907c 44 // Convert current GBW_dB into amplitude form
Padman 0:2e17a3d6907c 45 Bandwidth_gain_amplitude = pow(10, GBW_dB/20);
Padman 0:2e17a3d6907c 46
Padman 0:2e17a3d6907c 47 // Check this against the limits and clip if necessary
Padman 0:2e17a3d6907c 48 Bandwidth_gain_amplitude = PIN(Bandwidth_gain_amplitude, lower + GBW_MARGIN, upper - GBW_MARGIN);
Padman 0:2e17a3d6907c 49
Padman 0:2e17a3d6907c 50 // Convert the checked value back to decibel form
Padman 0:2e17a3d6907c 51 GBW_dB = 20*log10(Bandwidth_gain_amplitude);
Padman 0:2e17a3d6907c 52 }
Padman 0:2e17a3d6907c 53
Padman 0:2e17a3d6907c 54 void ParametricEQ::calculate() {
Padman 0:2e17a3d6907c 55
Padman 0:2e17a3d6907c 56 // Calculate sampling variables
Padman 0:2e17a3d6907c 57 Scaling_factor = 1 << (30 - Sample_bits);
Padman 0:2e17a3d6907c 58 Sample_bounds = (1 << (Sample_bits - 1)) - 1;
Padman 0:2e17a3d6907c 59
Padman 0:2e17a3d6907c 60 // Calculate parameter variables
Padman 0:2e17a3d6907c 61 Normalised_centre_frequency = (F0_Hz*2*M_PI)/Sample_rate;
Padman 0:2e17a3d6907c 62 Normalised_bandwidth = (BW_Hz*2*M_PI)/Sample_rate;
Padman 0:2e17a3d6907c 63 f0_cosine = cos(Normalised_centre_frequency);
Padman 0:2e17a3d6907c 64 f0_sine = sin(Normalised_centre_frequency);
Padman 0:2e17a3d6907c 65 g = pow(Gain_amplitude,double(1)/Order);
Padman 0:2e17a3d6907c 66 g_squared = g*g;
Padman 0:2e17a3d6907c 67 g0 = pow(G0_amplitude,double(1)/Order);
Padman 0:2e17a3d6907c 68 g0_squared = g0*g0;
Padman 0:2e17a3d6907c 69 epsilon = sqrt((Gain_amplitude*Gain_amplitude - Bandwidth_gain_amplitude*Bandwidth_gain_amplitude)/
Padman 0:2e17a3d6907c 70 (Bandwidth_gain_amplitude*Bandwidth_gain_amplitude - G0_amplitude*G0_amplitude));
Padman 0:2e17a3d6907c 71 beta = tan(Normalised_bandwidth/2)/pow(epsilon,double(1)/Order);
Padman 0:2e17a3d6907c 72 beta_squared = beta*beta;
Padman 0:2e17a3d6907c 73 counter = (Order + 1)/2;
Padman 0:2e17a3d6907c 74
Padman 0:2e17a3d6907c 75 update_blocks();
Padman 0:2e17a3d6907c 76 }
Padman 0:2e17a3d6907c 77
Padman 0:2e17a3d6907c 78 void ParametricEQ::update_blocks() {
Padman 0:2e17a3d6907c 79
Padman 0:2e17a3d6907c 80 // Iterate through filter blocks
Padman 0:2e17a3d6907c 81 for (int i = 0; i < counter; i++)
Padman 0:2e17a3d6907c 82 {
Padman 0:2e17a3d6907c 83 // Assign new sampling parameters
Padman 0:2e17a3d6907c 84 fx_blocks[i].scaling_factor = Scaling_factor;
Padman 0:2e17a3d6907c 85 fx_blocks[i].sample_bounds = Sample_bounds;
Padman 0:2e17a3d6907c 86
Padman 0:2e17a3d6907c 87 // Assign new coefficients
Padman 0:2e17a3d6907c 88 fx_blocks[i].c0 = f0_cosine*Scaling_factor;
Padman 0:2e17a3d6907c 89 fx_blocks[i].s0 = f0_sine*Scaling_factor;
Padman 0:2e17a3d6907c 90
Padman 0:2e17a3d6907c 91 if ((Order%2 == 1) && (i == 0))
Padman 0:2e17a3d6907c 92 {
Padman 0:2e17a3d6907c 93 D = beta + 1;
Padman 0:2e17a3d6907c 94 fx_blocks[i].bhat_0 = ((g*beta + g0)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 95 fx_blocks[i].bhat_1 = ((g*beta - g0)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 96 fx_blocks[i].bhat_2 = 0;
Padman 0:2e17a3d6907c 97 fx_blocks[i].ahat_1 = ((beta - 1)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 98 fx_blocks[i].ahat_2 = 0;
Padman 0:2e17a3d6907c 99 }
Padman 0:2e17a3d6907c 100
Padman 0:2e17a3d6907c 101 else
Padman 0:2e17a3d6907c 102 {
Padman 0:2e17a3d6907c 103 phi = (((2*i) + 1)*M_PI)/(2*Order);
Padman 0:2e17a3d6907c 104 si = sin(double(phi));
Padman 0:2e17a3d6907c 105 D = beta_squared + 2*si*beta + 1;
Padman 0:2e17a3d6907c 106 fx_blocks[i].bhat_0 = ((g_squared*beta_squared + 2*g*g0*si*beta + g0_squared)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 107 fx_blocks[i].bhat_1 = (2*(g_squared*beta_squared - g0_squared)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 108 fx_blocks[i].bhat_2 = ((g_squared*beta_squared - 2*g*g0*si*beta + g0_squared)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 109 fx_blocks[i].ahat_1 = (2*(beta_squared - 1)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 110 fx_blocks[i].ahat_2 = ((beta_squared - 2*si*beta + 1)/D)*Scaling_factor;
Padman 0:2e17a3d6907c 111 }
Padman 0:2e17a3d6907c 112 }
Padman 0:2e17a3d6907c 113 }
Padman 0:2e17a3d6907c 114
Padman 0:2e17a3d6907c 115 ParametricEQ::ParametricEQ() {
Padman 0:2e17a3d6907c 116
Padman 0:2e17a3d6907c 117 // Initial parameter values
Padman 0:2e17a3d6907c 118 Gain_dB = 6;
Padman 0:2e17a3d6907c 119 GBW_dB = 3;
Padman 0:2e17a3d6907c 120 F0_Hz = 4000;
Padman 0:2e17a3d6907c 121 BW_Hz = 1000;
Padman 0:2e17a3d6907c 122 Order = 2;
Padman 0:2e17a3d6907c 123 Type = Peaking;
Padman 0:2e17a3d6907c 124
Padman 0:2e17a3d6907c 125 // Initial sampling values
Padman 0:2e17a3d6907c 126 Sample_rate = 48000;
Padman 0:2e17a3d6907c 127 Sample_bits = 16;
Padman 0:2e17a3d6907c 128
Padman 0:2e17a3d6907c 129 // Initialise filter blocks
Padman 0:2e17a3d6907c 130 calculate();
Padman 0:2e17a3d6907c 131 }
Padman 0:2e17a3d6907c 132
Padman 0:2e17a3d6907c 133 float ParametricEQ::set_Gain_dB(float value) {
Padman 0:2e17a3d6907c 134
Padman 0:2e17a3d6907c 135 Gain_dB = PIN(value, -GAIN_DB_MAX, GAIN_DB_MAX);
Padman 0:2e17a3d6907c 136
Padman 0:2e17a3d6907c 137 set_Type(Type);
Padman 0:2e17a3d6907c 138 return Gain_dB;
Padman 0:2e17a3d6907c 139 }
Padman 0:2e17a3d6907c 140
Padman 0:2e17a3d6907c 141 float ParametricEQ::set_GBW_dB(float value) {
Padman 0:2e17a3d6907c 142
Padman 0:2e17a3d6907c 143 GBW_dB = PIN(value, -GAIN_DB_MAX, GAIN_DB_MAX);
Padman 0:2e17a3d6907c 144
Padman 0:2e17a3d6907c 145 set_Type(Type);
Padman 0:2e17a3d6907c 146 return GBW_dB;
Padman 0:2e17a3d6907c 147 }
Padman 0:2e17a3d6907c 148
Padman 0:2e17a3d6907c 149 int ParametricEQ::set_F0_Hz(int value) {
Padman 0:2e17a3d6907c 150
Padman 0:2e17a3d6907c 151 F0_Hz = PIN(value, 0, Sample_rate/2);
Padman 0:2e17a3d6907c 152
Padman 0:2e17a3d6907c 153 set_Type(Type);
Padman 0:2e17a3d6907c 154 return F0_Hz;
Padman 0:2e17a3d6907c 155 }
Padman 0:2e17a3d6907c 156
Padman 0:2e17a3d6907c 157 int ParametricEQ::set_BW_Hz(int value) {
Padman 0:2e17a3d6907c 158
Padman 0:2e17a3d6907c 159 BW_Hz = PIN(value, BW_HZ_MIN, Sample_rate/2);
Padman 0:2e17a3d6907c 160
Padman 0:2e17a3d6907c 161 set_Type(Type);
Padman 0:2e17a3d6907c 162 return BW_Hz;
Padman 0:2e17a3d6907c 163 }
Padman 0:2e17a3d6907c 164
Padman 0:2e17a3d6907c 165 int ParametricEQ::set_Order(int value) {
Padman 0:2e17a3d6907c 166
Padman 0:2e17a3d6907c 167 Order = PIN(value, 1, MAX_ORDER);
Padman 0:2e17a3d6907c 168
Padman 0:2e17a3d6907c 169 set_Type(Type);
Padman 0:2e17a3d6907c 170 return Order;
Padman 0:2e17a3d6907c 171 }
Padman 0:2e17a3d6907c 172
Padman 0:2e17a3d6907c 173 FilterType ParametricEQ::set_Type(FilterType type) {
Padman 0:2e17a3d6907c 174
Padman 0:2e17a3d6907c 175 Type = type;
Padman 0:2e17a3d6907c 176
Padman 0:2e17a3d6907c 177 // Tailor parameters to suite the chosen filter type
Padman 0:2e17a3d6907c 178 switch (Type) {
Padman 0:2e17a3d6907c 179
Padman 0:2e17a3d6907c 180 case Peaking:
Padman 0:2e17a3d6907c 181 Gain_amplitude = pow(10, Gain_dB/20);
Padman 0:2e17a3d6907c 182 G0_amplitude = 1;
Padman 0:2e17a3d6907c 183 break;
Padman 0:2e17a3d6907c 184
Padman 0:2e17a3d6907c 185 case BandPass:
Padman 0:2e17a3d6907c 186 Gain_amplitude = 1;
Padman 0:2e17a3d6907c 187 G0_amplitude = 0;
Padman 0:2e17a3d6907c 188 break;
Padman 0:2e17a3d6907c 189
Padman 0:2e17a3d6907c 190 case BandStop:
Padman 0:2e17a3d6907c 191 Gain_amplitude = 0;
Padman 0:2e17a3d6907c 192 G0_amplitude = 1;
Padman 0:2e17a3d6907c 193 break;
Padman 0:2e17a3d6907c 194
Padman 0:2e17a3d6907c 195 case LowShelf:
Padman 0:2e17a3d6907c 196 Gain_amplitude = pow(10, Gain_dB/20);
Padman 0:2e17a3d6907c 197 G0_amplitude = 1;
Padman 0:2e17a3d6907c 198 F0_Hz = 0;
Padman 0:2e17a3d6907c 199 break;
Padman 0:2e17a3d6907c 200
Padman 0:2e17a3d6907c 201 case HighShelf:
Padman 0:2e17a3d6907c 202 Gain_amplitude = pow(10, Gain_dB/20);
Padman 0:2e17a3d6907c 203 G0_amplitude = 1;
Padman 0:2e17a3d6907c 204 F0_Hz = Sample_rate/2;
Padman 0:2e17a3d6907c 205 break;
Padman 0:2e17a3d6907c 206
Padman 0:2e17a3d6907c 207 case LowPass:
Padman 0:2e17a3d6907c 208 Gain_amplitude = 1;
Padman 0:2e17a3d6907c 209 G0_amplitude = 0;
Padman 0:2e17a3d6907c 210 F0_Hz = 0;
Padman 0:2e17a3d6907c 211 break;
Padman 0:2e17a3d6907c 212
Padman 0:2e17a3d6907c 213 case HighPass:
Padman 0:2e17a3d6907c 214 Gain_amplitude = 1;
Padman 0:2e17a3d6907c 215 G0_amplitude = 0;
Padman 0:2e17a3d6907c 216 F0_Hz = Sample_rate/2;
Padman 0:2e17a3d6907c 217 break;
Padman 0:2e17a3d6907c 218 }
Padman 0:2e17a3d6907c 219
Padman 0:2e17a3d6907c 220 check_GBW();
Padman 0:2e17a3d6907c 221 calculate();
Padman 0:2e17a3d6907c 222 return Type;
Padman 0:2e17a3d6907c 223 }
Padman 0:2e17a3d6907c 224
Padman 0:2e17a3d6907c 225 int ParametricEQ::set_Sample_rate(int value) {
Padman 0:2e17a3d6907c 226
Padman 0:2e17a3d6907c 227 Sample_rate = PIN(value, SAMPLE_RATE_MIN, SAMPLE_RATE_MAX);
Padman 0:2e17a3d6907c 228
Padman 0:2e17a3d6907c 229 set_Type(Type);
Padman 0:2e17a3d6907c 230 return Sample_rate;
Padman 0:2e17a3d6907c 231 }
Padman 0:2e17a3d6907c 232
Padman 0:2e17a3d6907c 233 int ParametricEQ::set_Sample_bits(int value) {
Padman 0:2e17a3d6907c 234
Padman 0:2e17a3d6907c 235 Sample_bits = PIN(value, SAMPLE_BITS_MIN, SAMPLE_BITS_MAX);
Padman 0:2e17a3d6907c 236
Padman 0:2e17a3d6907c 237 set_Type(Type);
Padman 0:2e17a3d6907c 238 return Sample_bits;
Padman 0:2e17a3d6907c 239 }
Padman 0:2e17a3d6907c 240
Padman 0:2e17a3d6907c 241 int ParametricEQ::filter(int input) {
Padman 0:2e17a3d6907c 242
Padman 0:2e17a3d6907c 243 // Send sample through filter blocks
Padman 0:2e17a3d6907c 244 for (int i = 0; i < counter; i++)
Padman 0:2e17a3d6907c 245 {
Padman 0:2e17a3d6907c 246 input = fx_blocks[i].execute(input);
Padman 0:2e17a3d6907c 247 }
Padman 0:2e17a3d6907c 248
Padman 0:2e17a3d6907c 249 return input;
Padman 0:2e17a3d6907c 250 }
Padman 0:2e17a3d6907c 251