LVEQNB_CalcCoef.c revision 2c8e5cab3faa6d360e222b7a6c40a80083d021ac
1/* 2 * Copyright (C) 2004-2010 NXP Software 3 * Copyright (C) 2010 The Android Open Source Project 4 * 5 * Licensed under the Apache License, Version 2.0 (the "License"); 6 * you may not use this file except in compliance with the License. 7 * You may obtain a copy of the License at 8 * 9 * http://www.apache.org/licenses/LICENSE-2.0 10 * 11 * Unless required by applicable law or agreed to in writing, software 12 * distributed under the License is distributed on an "AS IS" BASIS, 13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 * See the License for the specific language governing permissions and 15 * limitations under the License. 16 */ 17 18/********************************************************************************** 19 20 $Author: beq07716 $ 21 $Revision: 1005 $ 22 $Date: 2010-06-28 13:58:28 +0200 (Mon, 28 Jun 2010) $ 23 24***********************************************************************************/ 25 26/****************************************************************************************/ 27/* */ 28/* Includes */ 29/* */ 30/****************************************************************************************/ 31 32#include "LVEQNB_Private.h" 33 34 35/****************************************************************************************/ 36/* */ 37/* Defines */ 38/* */ 39/****************************************************************************************/ 40 41#define PI 3.14159265358979 42 43/****************************************************************************************/ 44/* */ 45/* FUNCTION: LVEQNB_DoublePrecCoefs */ 46/* */ 47/* DESCRIPTION: */ 48/* Calculate double precision coefficients for a peaking filter */ 49/* */ 50/* PARAMETERS: */ 51/* Fs Sampling frequency index */ 52/* pFilterDefinition Pointer to the filter definition */ 53/* pCoefficients Pointer to the coefficients */ 54/* */ 55/* RETURNS: */ 56/* LVEQNB_SUCCESS Always succeeds */ 57/* */ 58/* NOTES: */ 59/* 1. The equations used are as follows: */ 60/* */ 61/* G = 10^(GaindB/20) - 1 */ 62/* t0 = 2 * Pi * Fc / Fs */ 63/* D = 1 if GaindB >= 0 */ 64/* D = 1 / (1 + G) if GaindB < 0 */ 65/* */ 66/* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ 67/* b1 = (0.5 - b2) * (1 - coserr(t0)) */ 68/* a0 = (0.5 + b2) / 2 */ 69/* */ 70/* Where: */ 71/* GaindB is the gain in dBs, range -15dB to +15dB */ 72/* Fc is the centre frequency, DC to Fs/50 */ 73/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 74/* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */ 75/* */ 76/* 2. The double precision coefficients are only used when fc is less than fs/85, so */ 77/* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */ 78/* itself the difference from the value 1.0 is calculated, this can be done with */ 79/* lower precision maths. */ 80/* */ 81/* 3. The value of the B2 coefficient is only calculated as a single precision value, */ 82/* small errors in this value have a combined effect on the Q and Gain but not the */ 83/* the frequency of the filter. */ 84/* */ 85/****************************************************************************************/ 86 87 88LVEQNB_ReturnStatus_en LVEQNB_DoublePrecCoefs(LVM_UINT16 Fs, 89 LVEQNB_BandDef_t *pFilterDefinition, 90 PK_C32_Coefs_t *pCoefficients) 91{ 92 93 extern LVM_INT16 LVEQNB_GainTable[]; 94 extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; 95 extern LVM_INT16 LVEQNB_DTable[]; 96 extern LVM_INT16 LVEQNB_DPCosCoef[]; 97 98 /* 99 * Get the filter definition 100 */ 101 LVM_INT16 Gain = pFilterDefinition->Gain; 102 LVM_UINT16 Frequency = pFilterDefinition->Frequency; 103 LVM_UINT16 QFactor = pFilterDefinition->QFactor; 104 105 /* 106 * Intermediate variables and temporary values 107 */ 108 LVM_INT32 T0; 109 LVM_INT16 D; 110 LVM_INT32 A0; 111 LVM_INT32 B1; 112 LVM_INT32 B2; 113 LVM_INT32 Dt0; 114 LVM_INT32 B2_Den; 115 LVM_INT32 B2_Num; 116 LVM_INT32 CosErr; 117 LVM_INT16 coef; 118 LVM_INT32 factor; 119 LVM_INT16 t0; 120 LVM_INT16 i; 121 122 /* 123 * Calculating the intermediate values 124 */ 125 T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 126 if (Gain >= 0) 127 { 128 D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ 129 } 130 else 131 { 132 D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ 133 } 134 135 /* 136 * Calculate the B2 coefficient 137 */ 138 Dt0 = D * (T0 >> 10); 139 B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); 140 B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); 141 B2 = (B2_Num / (B2_Den >> 16)) << 15; 142 143 /* 144 * Calculate the cosine error by a polynomial expansion using the equation: 145 * 146 * CosErr += coef(n) * t0^n For n = 0 to 4 147 */ 148 T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ 149 t0 = (LVM_INT16)(T0 >> 16); 150 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 151 CosErr = 0; /* Initialise the error to zero */ 152 for (i=1; i<5; i++) 153 { 154 coef = LVEQNB_DPCosCoef[i]; /* Get the nth coefficient */ 155 CosErr += (factor * coef) >> 5; /* The nth partial sum */ 156 factor = (factor * t0) >> 15; /* Calculate t0^n */ 157 } 158 CosErr = CosErr << (LVEQNB_DPCosCoef[0]); /* Correct the scaling */ 159 160 /* 161 * Calculate the B1 and A0 coefficients 162 */ 163 B1 = (0x40000000 - B2); /* B1 = (0.5 - b2/2) */ 164 A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2/2) * coserr(t0) */ 165 B1 -= A0; /* B1 = (0.5 - b2/2) * (1 - coserr(t0)) */ 166 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) */ 167 168 /* 169 * Write coeff into the data structure 170 */ 171 pCoefficients->A0 = A0; 172 pCoefficients->B1 = B1; 173 pCoefficients->B2 = B2; 174 pCoefficients->G = LVEQNB_GainTable[Gain+15]; 175 176 return(LVEQNB_SUCCESS); 177 178} 179 180 181/****************************************************************************************/ 182/* */ 183/* FUNCTION: LVEQNB_SinglePrecCoefs */ 184/* */ 185/* DESCRIPTION: */ 186/* Calculate single precision coefficients for a peaking filter */ 187/* */ 188/* PARAMETERS: */ 189/* Fs Sampling frequency index */ 190/* pFilterDefinition Pointer to the filter definition */ 191/* pCoefficients Pointer to the coefficients */ 192/* */ 193/* RETURNS: */ 194/* LVEQNB_SUCCESS Always succeeds */ 195/* */ 196/* NOTES: */ 197/* 1. The equations used are as follows: */ 198/* */ 199/* G = 10^(GaindB/20) - 1 */ 200/* t0 = 2 * Pi * Fc / Fs */ 201/* D = 1 if GaindB >= 0 */ 202/* D = 1 / (1 + G) if GaindB < 0 */ 203/* */ 204/* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ 205/* b1 = (0.5 - b2) * cos(t0) */ 206/* a0 = (0.5 + b2) / 2 */ 207/* */ 208/* Where: */ 209/* GaindB is the gain in dBs, range -15dB to +15dB */ 210/* Fc is the centre frequency, DC to Nyquist */ 211/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 212/* Q is the Q factor, 0.25 to 12 */ 213/* */ 214/****************************************************************************************/ 215 216 217LVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs, 218 LVEQNB_BandDef_t *pFilterDefinition, 219 PK_C16_Coefs_t *pCoefficients) 220{ 221 222 extern LVM_INT16 LVEQNB_GainTable[]; 223 extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; 224 extern LVM_INT16 LVEQNB_DTable[]; 225 extern LVM_INT16 LVEQNB_CosCoef[]; 226 227 228 /* 229 * Get the filter definition 230 */ 231 LVM_INT16 Gain = pFilterDefinition->Gain; 232 LVM_UINT16 Frequency = pFilterDefinition->Frequency; 233 LVM_UINT16 QFactor = pFilterDefinition->QFactor; 234 235 236 /* 237 * Intermediate variables and temporary values 238 */ 239 LVM_INT32 T0; 240 LVM_INT16 D; 241 LVM_INT32 A0; 242 LVM_INT32 B1; 243 LVM_INT32 B2; 244 LVM_INT32 Dt0; 245 LVM_INT32 B2_Den; 246 LVM_INT32 B2_Num; 247 LVM_INT32 COS_T0; 248 LVM_INT16 coef; 249 LVM_INT32 factor; 250 LVM_INT16 t0; 251 LVM_INT16 i; 252 253 /* 254 * Calculating the intermediate values 255 */ 256 T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 257 if (Gain >= 0) 258 { 259 D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ 260 } 261 else 262 { 263 D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ 264 } 265 266 /* 267 * Calculate the B2 coefficient 268 */ 269 Dt0 = D * (T0 >> 10); 270 B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); 271 B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); 272 B2 = (B2_Num / (B2_Den >> 16)) << 15; 273 274 /* 275 * Calculate the cosine by a polynomial expansion using the equation: 276 * 277 * Cos += coef(n) * t0^n For n = 0 to 6 278 */ 279 T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ 280 t0 = (LVM_INT16)(T0 >> 16); 281 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 282 COS_T0 = 0; /* Initialise the error to zero */ 283 for (i=1; i<7; i++) 284 { 285 coef = LVEQNB_CosCoef[i]; /* Get the nth coefficient */ 286 COS_T0 += (factor * coef) >> 5; /* The nth partial sum */ 287 factor = (factor * t0) >> 15; /* Calculate t0^n */ 288 } 289 COS_T0 = COS_T0 << (LVEQNB_CosCoef[0]+6); /* Correct the scaling */ 290 291 292 B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2/2) * cos(t0) */ 293 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2/2) */ 294 295 /* 296 * Write coeff into the data structure 297 */ 298 pCoefficients->A0 = (LVM_INT16)(A0>>16); 299 pCoefficients->B1 = (LVM_INT16)(B1>>15); 300 pCoefficients->B2 = (LVM_INT16)(B2>>16); 301 pCoefficients->G = LVEQNB_GainTable[Gain+15]; 302 303 304 return(LVEQNB_SUCCESS); 305 306} 307