12c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* 22c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Copyright (C) 2004-2010 NXP Software 32c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Copyright (C) 2010 The Android Open Source Project 42c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * 52c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Licensed under the Apache License, Version 2.0 (the "License"); 62c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * you may not use this file except in compliance with the License. 72c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * You may obtain a copy of the License at 82c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * 92c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * http://www.apache.org/licenses/LICENSE-2.0 102c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * 112c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Unless required by applicable law or agreed to in writing, software 122c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * distributed under the License is distributed on an "AS IS" BASIS, 132c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 142c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * See the License for the specific language governing permissions and 152c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * limitations under the License. 162c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 172c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 182c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 192c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 202c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Includes */ 212c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 222c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 232c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 242c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent#include "LVEQNB_Private.h" 252c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 262c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 272c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 282c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 292c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Defines */ 302c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 312c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 322c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 332c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent#define PI 3.14159265358979 342c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 352c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 362c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 372c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* FUNCTION: LVEQNB_DoublePrecCoefs */ 382c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 392c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* DESCRIPTION: */ 402c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Calculate double precision coefficients for a peaking filter */ 412c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 422c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* PARAMETERS: */ 432c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fs Sampling frequency index */ 442c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* pFilterDefinition Pointer to the filter definition */ 452c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* pCoefficients Pointer to the coefficients */ 462c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 472c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* RETURNS: */ 482c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* LVEQNB_SUCCESS Always succeeds */ 492c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 502c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* NOTES: */ 512c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* 1. The equations used are as follows: */ 522c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 532c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* G = 10^(GaindB/20) - 1 */ 542c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* t0 = 2 * Pi * Fc / Fs */ 552c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* D = 1 if GaindB >= 0 */ 562c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* D = 1 / (1 + G) if GaindB < 0 */ 572c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 582c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ 592c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* b1 = (0.5 - b2) * (1 - coserr(t0)) */ 602c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* a0 = (0.5 + b2) / 2 */ 612c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 622c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Where: */ 632c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* GaindB is the gain in dBs, range -15dB to +15dB */ 642c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fc is the centre frequency, DC to Fs/50 */ 652c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 662c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */ 672c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 682c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* 2. The double precision coefficients are only used when fc is less than fs/85, so */ 692c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */ 702c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* itself the difference from the value 1.0 is calculated, this can be done with */ 712c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* lower precision maths. */ 722c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 732c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* 3. The value of the B2 coefficient is only calculated as a single precision value, */ 742c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* small errors in this value have a combined effect on the Q and Gain but not the */ 752c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* the frequency of the filter. */ 762c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 772c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 782c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 792c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 802c8e5cab3faa6d360e222b7a6c40a80083d021acEric LaurentLVEQNB_ReturnStatus_en LVEQNB_DoublePrecCoefs(LVM_UINT16 Fs, 812c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVEQNB_BandDef_t *pFilterDefinition, 822c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent PK_C32_Coefs_t *pCoefficients) 832c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent{ 842c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 852c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_GainTable[]; 862c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; 872c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_DTable[]; 882c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_DPCosCoef[]; 892c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 902c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 912c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Get the filter definition 922c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 932c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 Gain = pFilterDefinition->Gain; 942c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_UINT16 Frequency = pFilterDefinition->Frequency; 952c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_UINT16 QFactor = pFilterDefinition->QFactor; 962c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 972c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 982c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Intermediate variables and temporary values 992c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1002c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 T0; 1012c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 D; 1022c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 A0; 1032c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B1; 1042c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2; 1052c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 Dt0; 1062c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2_Den; 1072c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2_Num; 1082c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 CosErr; 1092c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 coef; 1102c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 factor; 1112c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 t0; 1122c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 i; 1132c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1142c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 1152c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculating the intermediate values 1162c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1172c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 1182c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent if (Gain >= 0) 1192c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 1202c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ 1212c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 1222c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent else 1232c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 1242c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ 1252c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 1262c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1272c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 1282c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculate the B2 coefficient 1292c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1302c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent Dt0 = D * (T0 >> 10); 1312c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); 1322c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); 1332c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2 = (B2_Num / (B2_Den >> 16)) << 15; 1342c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1352c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 1362c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculate the cosine error by a polynomial expansion using the equation: 1372c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * 1382c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * CosErr += coef(n) * t0^n For n = 0 to 4 1392c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1402c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ 1412c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent t0 = (LVM_INT16)(T0 >> 16); 1422c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 1432c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent CosErr = 0; /* Initialise the error to zero */ 1442c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent for (i=1; i<5; i++) 1452c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 1462c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent coef = LVEQNB_DPCosCoef[i]; /* Get the nth coefficient */ 1472c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent CosErr += (factor * coef) >> 5; /* The nth partial sum */ 1482c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent factor = (factor * t0) >> 15; /* Calculate t0^n */ 1492c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 1502c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent CosErr = CosErr << (LVEQNB_DPCosCoef[0]); /* Correct the scaling */ 1512c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1522c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 1532c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculate the B1 and A0 coefficients 1542c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1552c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B1 = (0x40000000 - B2); /* B1 = (0.5 - b2/2) */ 1562c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2/2) * coserr(t0) */ 1572c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B1 -= A0; /* B1 = (0.5 - b2/2) * (1 - coserr(t0)) */ 1582c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) */ 1592c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1602c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 1612c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Write coeff into the data structure 1622c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 1632c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->A0 = A0; 1642c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->B1 = B1; 1652c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->B2 = B2; 1662c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->G = LVEQNB_GainTable[Gain+15]; 1672c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1682c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent return(LVEQNB_SUCCESS); 1692c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1702c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent} 1712c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1722c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 1732c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 1742c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1752c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* FUNCTION: LVEQNB_SinglePrecCoefs */ 1762c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1772c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* DESCRIPTION: */ 1782c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Calculate single precision coefficients for a peaking filter */ 1792c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1802c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* PARAMETERS: */ 1812c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fs Sampling frequency index */ 1822c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* pFilterDefinition Pointer to the filter definition */ 1832c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* pCoefficients Pointer to the coefficients */ 1842c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1852c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* RETURNS: */ 1862c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* LVEQNB_SUCCESS Always succeeds */ 1872c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1882c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* NOTES: */ 1892c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* 1. The equations used are as follows: */ 1902c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1912c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* G = 10^(GaindB/20) - 1 */ 1922c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* t0 = 2 * Pi * Fc / Fs */ 1932c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* D = 1 if GaindB >= 0 */ 1942c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* D = 1 / (1 + G) if GaindB < 0 */ 1952c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 1962c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* b2 = -0.5 * (2Q - D * t0) / (2Q + D * t0) */ 1972c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* b1 = (0.5 - b2) * cos(t0) */ 1982c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* a0 = (0.5 + b2) / 2 */ 1992c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 2002c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Where: */ 2012c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* GaindB is the gain in dBs, range -15dB to +15dB */ 2022c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fc is the centre frequency, DC to Nyquist */ 2032c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 2042c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* Q is the Q factor, 0.25 to 12 */ 2052c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/* */ 2062c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent/****************************************************************************************/ 2072c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2082c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2092c8e5cab3faa6d360e222b7a6c40a80083d021acEric LaurentLVEQNB_ReturnStatus_en LVEQNB_SinglePrecCoefs(LVM_UINT16 Fs, 2102c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVEQNB_BandDef_t *pFilterDefinition, 2112c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent PK_C16_Coefs_t *pCoefficients) 2122c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent{ 2132c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2142c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_GainTable[]; 2152c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_TwoPiOnFsTable[]; 2162c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_DTable[]; 2172c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent extern LVM_INT16 LVEQNB_CosCoef[]; 2182c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2192c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2202c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2212c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Get the filter definition 2222c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2232c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 Gain = pFilterDefinition->Gain; 2242c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_UINT16 Frequency = pFilterDefinition->Frequency; 2252c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_UINT16 QFactor = pFilterDefinition->QFactor; 2262c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2272c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2282c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2292c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Intermediate variables and temporary values 2302c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2312c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 T0; 2322c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 D; 2332c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 A0; 2342c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B1; 2352c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2; 2362c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 Dt0; 2372c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2_Den; 2382c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 B2_Num; 2392c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 COS_T0; 2402c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 coef; 2412c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT32 factor; 2422c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 t0; 2432c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent LVM_INT16 i; 2442c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2452c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2462c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculating the intermediate values 2472c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2482c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent T0 = (LVM_INT32)Frequency * LVEQNB_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 2492c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent if (Gain >= 0) 2502c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 2512c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent D = LVEQNB_DTable[15]; /* D = 1 if GaindB >= 0 */ 2522c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 2532c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent else 2542c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 2552c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent D = LVEQNB_DTable[Gain+15]; /* D = 1 / (1 + G) if GaindB < 0 */ 2562c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 2572c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2582c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2592c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculate the B2 coefficient 2602c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2612c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent Dt0 = D * (T0 >> 10); 2622c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2_Den = ((LVM_INT32)QFactor << 19) + (Dt0 >> 2); 2632c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2_Num = (Dt0 >> 3) - ((LVM_INT32)QFactor << 18); 2642c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B2 = (B2_Num / (B2_Den >> 16)) << 15; 2652c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2662c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2672c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Calculate the cosine by a polynomial expansion using the equation: 2682c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * 2692c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Cos += coef(n) * t0^n For n = 0 to 6 2702c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2712c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ 2722c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent t0 = (LVM_INT16)(T0 >> 16); 2732c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 2742c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent COS_T0 = 0; /* Initialise the error to zero */ 2752c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent for (i=1; i<7; i++) 2762c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent { 2772c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent coef = LVEQNB_CosCoef[i]; /* Get the nth coefficient */ 2782c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent COS_T0 += (factor * coef) >> 5; /* The nth partial sum */ 2792c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent factor = (factor * t0) >> 15; /* Calculate t0^n */ 2802c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent } 2812c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent COS_T0 = COS_T0 << (LVEQNB_CosCoef[0]+6); /* Correct the scaling */ 2822c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2832c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2842c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2/2) * cos(t0) */ 2852c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2/2) */ 2862c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2872c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent /* 2882c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent * Write coeff into the data structure 2892c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent */ 2902c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->A0 = (LVM_INT16)(A0>>16); 2912c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->B1 = (LVM_INT16)(B1>>15); 2922c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->B2 = (LVM_INT16)(B2>>16); 2932c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent pCoefficients->G = LVEQNB_GainTable[Gain+15]; 2942c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2952c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2962c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent return(LVEQNB_SUCCESS); 2972c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent 2982c8e5cab3faa6d360e222b7a6c40a80083d021acEric Laurent} 299