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#include "LVPSA.h" 19#include "LVPSA_Private.h" 20#include "VectorArithmetic.h" 21 22#define LOW_FREQ 298 /* 32768/110 for low test frequency */ 23#define HIGH_FREQ 386 /* 32768/85 for high test frequency */ 24 25LVPSA_RETURN LVPSA_SetBPFiltersType ( LVPSA_InstancePr_t *pInst, 26 LVPSA_ControlParams_t *pParams ); 27 28LVPSA_RETURN LVPSA_SetQPFCoefficients( LVPSA_InstancePr_t *pInst, 29 LVPSA_ControlParams_t *pParams ); 30 31LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 32 LVPSA_FilterParam_t *pFilterParams, 33 BP_C16_Coefs_t *pCoefficients); 34 35LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 36 LVPSA_FilterParam_t *pFilterParams, 37 BP_C32_Coefs_t *pCoefficients); 38 39LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 40 LVPSA_FilterParam_t *pFilterParams, 41 BP_C32_Coefs_t *pCoefficients); 42 43LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst, 44 LVPSA_ControlParams_t *pParams ); 45 46LVPSA_RETURN LVPSA_ClearFilterHistory( LVPSA_InstancePr_t *pInst); 47 48 49 50 51/************************************************************************************/ 52/* */ 53/* FUNCTION: LVPSA_Control */ 54/* */ 55/* DESCRIPTION: */ 56/* Give some new control parameters to the module. */ 57/* */ 58/* PARAMETERS: */ 59/* hInstance Pointer to the instance */ 60/* NewParams Structure that contains the new parameters */ 61/* */ 62/* RETURNS: */ 63/* LVPSA_OK Succeeds */ 64/* otherwise Error due to bad parameters */ 65/* */ 66/************************************************************************************/ 67LVPSA_RETURN LVPSA_Control ( pLVPSA_Handle_t hInstance, 68 LVPSA_ControlParams_t *pNewParams ) 69{ 70 71 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 72 73 if((hInstance == LVM_NULL) || (pNewParams == LVM_NULL)) 74 { 75 return(LVPSA_ERROR_NULLADDRESS); 76 } 77 if(pNewParams->Fs >= LVPSA_NR_SUPPORTED_RATE) 78 { 79 return(LVPSA_ERROR_INVALIDPARAM); 80 } 81 if(pNewParams->LevelDetectionSpeed >= LVPSA_NR_SUPPORTED_SPEED) 82 { 83 return(LVPSA_ERROR_INVALIDPARAM); 84 } 85 86 pLVPSA_Inst->NewParams = *pNewParams; 87 pLVPSA_Inst->bControlPending = LVM_TRUE; 88 89 return(LVPSA_OK); 90} 91 92/************************************************************************************/ 93/* */ 94/* FUNCTION: LVPSA_GetControlParams */ 95/* */ 96/* DESCRIPTION: */ 97/* Get the current control parameters of the module */ 98/* */ 99/* PARAMETERS: */ 100/* hInstance Pointer to the instance */ 101/* pParams Pointer to an empty control structure */ 102/* RETURNS: */ 103/* LVPSA_OK Succeeds */ 104/* otherwise Error due to bad parameters */ 105/* */ 106/************************************************************************************/ 107LVPSA_RETURN LVPSA_GetControlParams ( pLVPSA_Handle_t hInstance, 108 LVPSA_ControlParams_t *pParams ) 109{ 110 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 111 112 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 113 { 114 return(LVPSA_ERROR_NULLADDRESS); 115 } 116 117 pParams->Fs = pLVPSA_Inst->CurrentParams.Fs; 118 pParams->LevelDetectionSpeed = pLVPSA_Inst->CurrentParams.LevelDetectionSpeed; 119 120 return(LVPSA_OK); 121} 122 123 124/************************************************************************************/ 125/* */ 126/* FUNCTION: LVPSA_GetInitParams */ 127/* */ 128/* DESCRIPTION: */ 129/* Get the initialization parameters of the module */ 130/* */ 131/* PARAMETERS: */ 132/* hInstance Pointer to the instance */ 133/* pParams Pointer to an empty control structure */ 134/* RETURNS: */ 135/* LVPSA_OK Succeeds */ 136/* otherwise Error due to bad parameters */ 137/* */ 138/************************************************************************************/ 139LVPSA_RETURN LVPSA_GetInitParams ( pLVPSA_Handle_t hInstance, 140 LVPSA_InitParams_t *pParams ) 141{ 142 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 143 144 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 145 { 146 return(LVPSA_ERROR_NULLADDRESS); 147 } 148 149 pParams->SpectralDataBufferDuration = pLVPSA_Inst->SpectralDataBufferDuration; 150 pParams->MaxInputBlockSize = pLVPSA_Inst->MaxInputBlockSize; 151 pParams->nBands = pLVPSA_Inst->nBands; 152 pParams->pFiltersParams = pLVPSA_Inst->pFiltersParams; 153 154 return(LVPSA_OK); 155} 156 157 158/************************************************************************************/ 159/* */ 160/* FUNCTION: LVPSA_ApplyNewSettings */ 161/* */ 162/* DESCRIPTION: */ 163/* Reinitialize some parameters and changes filters' coefficients if */ 164/* some control parameters have changed. */ 165/* */ 166/* PARAMETERS: */ 167/* pInst Pointer to the instance */ 168/* */ 169/* RETURNS: */ 170/* LVPSA_OK Succeeds */ 171/* otherwise Error due to bad parameters */ 172/* */ 173/* NOTES: */ 174/* */ 175/************************************************************************************/ 176LVPSA_RETURN LVPSA_ApplyNewSettings (LVPSA_InstancePr_t *pInst) 177{ 178 LVM_UINT16 ii; 179 LVM_UINT16 Freq; 180 LVPSA_ControlParams_t Params; 181 extern LVM_INT16 LVPSA_nSamplesBufferUpdate[]; 182 extern LVM_UINT16 LVPSA_SampleRateTab[]; 183 extern LVM_UINT16 LVPSA_DownSamplingFactor[]; 184 185 186 if(pInst == 0) 187 { 188 return(LVPSA_ERROR_NULLADDRESS); 189 } 190 191 Params = pInst->NewParams; 192 193 /* Modifies filters types and coefficients, clear the taps and 194 re-initializes parameters if sample frequency has changed */ 195 if(Params.Fs != pInst->CurrentParams.Fs) 196 { 197 pInst->CurrentParams.Fs = Params.Fs; 198 199 /* Initialize the center freqeuncies as a function of the sample rate */ 200 Freq = (LVM_UINT16) ((LVPSA_SampleRateTab[pInst->CurrentParams.Fs]>>1) / (pInst->nBands + 1)); 201 for(ii = pInst->nBands; ii > 0; ii--) 202 { 203 pInst->pFiltersParams[ii-1].CenterFrequency = (LVM_UINT16) (Freq * ii); 204 } 205 206 /* Count the number of relevant filters. If the center frequency of the filter is 207 bigger than the nyquist frequency, then the filter is not relevant and doesn't 208 need to be used */ 209 for(ii = pInst->nBands; ii > 0; ii--) 210 { 211 if(pInst->pFiltersParams[ii-1].CenterFrequency < (LVPSA_SampleRateTab[pInst->CurrentParams.Fs]>>1)) 212 { 213 pInst->nRelevantFilters = ii; 214 break; 215 } 216 } 217 LVPSA_SetBPFiltersType(pInst, &Params); 218 LVPSA_SetBPFCoefficients(pInst, &Params); 219 LVPSA_SetQPFCoefficients(pInst, &Params); 220 LVPSA_ClearFilterHistory(pInst); 221 pInst->nSamplesBufferUpdate = (LVM_UINT16)LVPSA_nSamplesBufferUpdate[Params.Fs]; 222 pInst->BufferUpdateSamplesCount = 0; 223 pInst->DownSamplingFactor = LVPSA_DownSamplingFactor[Params.Fs]; 224 pInst->DownSamplingCount = 0; 225 for(ii = 0; ii < (pInst->nBands * pInst->SpectralDataBufferLength); ii++) 226 { 227 pInst->pSpectralDataBufferStart[ii] = 0; 228 } 229 for(ii = 0; ii < pInst->nBands; ii++) 230 { 231 pInst->pPreviousPeaks[ii] = 0; 232 } 233 } 234 else 235 { 236 if(Params.LevelDetectionSpeed != pInst->CurrentParams.LevelDetectionSpeed) 237 { 238 LVPSA_SetQPFCoefficients(pInst, &Params); 239 } 240 } 241 242 pInst->CurrentParams = pInst->NewParams; 243 244 return (LVPSA_OK); 245} 246/************************************************************************************/ 247/* */ 248/* FUNCTION: LVPSA_SetBPFiltersType */ 249/* */ 250/* DESCRIPTION: */ 251/* Sets the filter type based on the BPFilterType. */ 252/* */ 253/* PARAMETERS: */ 254/* pInst Pointer to the instance */ 255/* pParams Poniter to conrol parameters */ 256/* */ 257/* RETURNS: */ 258/* LVPSA_OK Always succeeds */ 259/* */ 260/* NOTES: */ 261/* 1. To select the biquad type the follow rules are applied: */ 262/* Double precision if (fc <= fs/110) */ 263/* Double precision if (fs/110 < fc < fs/85) & (Q>3) */ 264/* Single precision otherwise */ 265/* */ 266/************************************************************************************/ 267LVPSA_RETURN LVPSA_SetBPFiltersType ( LVPSA_InstancePr_t *pInst, 268 LVPSA_ControlParams_t *pParams ) 269{ 270 271 extern LVM_UINT16 LVPSA_SampleRateTab[]; /* Sample rate table */ 272 LVM_UINT16 ii; /* Filter band index */ 273 LVM_UINT32 fs = (LVM_UINT32)LVPSA_SampleRateTab[(LVM_UINT16)pParams->Fs]; /* Sample rate */ 274 LVM_UINT32 fc; /* Filter centre frequency */ 275 LVM_INT16 QFactor; /* Filter Q factor */ 276 277 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 278 { 279 /* 280 * Get the filter settings 281 */ 282 fc = (LVM_UINT32)pInst->pFiltersParams[ii].CenterFrequency; /* Get the band centre frequency */ 283 QFactor =(LVM_INT16) pInst->pFiltersParams[ii].QFactor; /* Get the band Q factor */ 284 285 286 /* 287 * For each filter set the type of biquad required 288 */ 289 pInst->pBPFiltersPrecision[ii] = LVPSA_SimplePrecisionFilter; /* Default to single precision */ 290 if ((LOW_FREQ * fs) >= (fc << 15)) 291 { 292 /* 293 * fc <= fs/110 294 */ 295 pInst->pBPFiltersPrecision[ii] = LVPSA_DoublePrecisionFilter; 296 } 297 else 298 { 299 if (((LOW_FREQ * fs) < (fc << 15)) && ((fc << 15) < (HIGH_FREQ * fs)) && (QFactor > 300)) 300 { 301 /* 302 * (fs/110 < fc < fs/85) & (Q>3) 303 */ 304 pInst->pBPFiltersPrecision[ii] = LVPSA_DoublePrecisionFilter; 305 } 306 } 307 } 308 309 return(LVPSA_OK); 310} 311 312/************************************************************************************/ 313/* */ 314/* FUNCTION: LVPSA_SetBPFCoefficients */ 315/* */ 316/* DESCRIPTION: */ 317/* Sets the band pass filter coefficients. This uses the type to select */ 318/* single or double precision coefficients. */ 319/* */ 320/* PARAMETERS: */ 321/* pInst Pointer to the instance */ 322/* Params Initialisation parameters */ 323/* */ 324/* RETURNS: */ 325/* LVPSA_OK Always succeeds */ 326/* */ 327/* NOTES: */ 328/* */ 329/************************************************************************************/ 330LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst, 331 LVPSA_ControlParams_t *pParams) 332{ 333 334 LVM_UINT16 ii; 335 336 /* 337 * Set the coefficients for each band by the init function 338 */ 339 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 340 { 341 switch (pInst->pBPFiltersPrecision[ii]) 342 { 343 case LVPSA_DoublePrecisionFilter: 344 { 345 BP_C32_Coefs_t Coefficients; 346 347 /* 348 * Calculate the double precision coefficients 349 */ 350 LVPSA_BPDoublePrecCoefs((LVM_UINT16)pParams->Fs, 351 &pInst->pFiltersParams[ii], 352 &Coefficients); 353 354 /* 355 * Set the coefficients 356 */ 357 BP_1I_D16F32Cll_TRC_WRA_01_Init ( &pInst->pBP_Instances[ii], 358 &pInst->pBP_Taps[ii], 359 &Coefficients); 360 break; 361 } 362 363 case LVPSA_SimplePrecisionFilter: 364 { 365 BP_C16_Coefs_t Coefficients; 366 367 /* 368 * Calculate the single precision coefficients 369 */ 370 LVPSA_BPSinglePrecCoefs((LVM_UINT16)pParams->Fs, 371 &pInst->pFiltersParams[ii], 372 &Coefficients); 373 374 /* 375 * Set the coefficients 376 */ 377 BP_1I_D16F16Css_TRC_WRA_01_Init ( &pInst->pBP_Instances[ii], 378 &pInst->pBP_Taps[ii], 379 &Coefficients); 380 break; 381 } 382 } 383 } 384 385 return(LVPSA_OK); 386} 387 388 389/************************************************************************************/ 390/* */ 391/* FUNCTION: LVPSA_SetQPFCoefficients */ 392/* */ 393/* DESCRIPTION: */ 394/* Sets the quasi peak filters coefficients. This uses the chosen */ 395/* LevelDetectionSpeed from the control parameters. */ 396/* */ 397/* PARAMETERS: */ 398/* pInst Pointer to the instance */ 399/* Params Control parameters */ 400/* */ 401/* RETURNS: */ 402/* LVPSA_OK Always succeeds */ 403/* */ 404/* NOTES: */ 405/* */ 406/************************************************************************************/ 407LVPSA_RETURN LVPSA_SetQPFCoefficients( LVPSA_InstancePr_t *pInst, 408 LVPSA_ControlParams_t *pParams ) 409{ 410 LVM_UINT16 ii; 411 LVM_Fs_en Fs = pParams->Fs; 412 QPD_C32_Coefs *pCoefficients; 413 extern QPD_C32_Coefs LVPSA_QPD_Coefs[]; 414 415 416 pCoefficients = &LVPSA_QPD_Coefs[(pParams->LevelDetectionSpeed * LVPSA_NR_SUPPORTED_RATE) + Fs]; 417 418 419 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 420 { 421 LVPSA_QPD_Init (&pInst->pQPD_States[ii], 422 &pInst->pQPD_Taps[ii], 423 pCoefficients ); 424 } 425 426 return(LVPSA_OK); 427 428} 429 430/****************************************************************************************/ 431/* */ 432/* FUNCTION: LVPSA_BPSinglePrecCoefs */ 433/* */ 434/* DESCRIPTION: */ 435/* Calculate single precision coefficients for a band pass filter */ 436/* */ 437/* PARAMETERS: */ 438/* Fs Sampling frequency index */ 439/* pFilterParams Pointer to the filter definition */ 440/* pCoefficients Pointer to the coefficients */ 441/* */ 442/* RETURNS: */ 443/* LVPSA_OK Always succeeds */ 444/* */ 445/* NOTES: */ 446/* 1. The equations used are as follows: */ 447/* */ 448/* t0 = 2 * Pi * Fc / Fs */ 449/* */ 450/* b2 = -0.5 * (2Q - t0) / (2Q + t0) */ 451/* b1 = (0.5 - b2) * cos(t0) */ 452/* a0 = (0.5 + b2) / 2 */ 453/* */ 454/* Where: */ 455/* Fc is the centre frequency, DC to Nyquist */ 456/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 457/* Q is the Q factor, 0.25 to 12 */ 458/* */ 459/* 2. This function is entirely based on the LVEQNB_SinglePrecCoefs function */ 460/* of the n bands equalizer (LVEQNB */ 461/* */ 462/****************************************************************************************/ 463LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 464 LVPSA_FilterParam_t *pFilterParams, 465 BP_C16_Coefs_t *pCoefficients) 466{ 467 468 extern LVM_INT16 LVPSA_TwoPiOnFsTable[]; 469 extern LVM_INT16 LVPSA_CosCoef[]; 470 471 472 /* 473 * Intermediate variables and temporary values 474 */ 475 LVM_INT32 T0; 476 LVM_INT16 D; 477 LVM_INT32 A0; 478 LVM_INT32 B1; 479 LVM_INT32 B2; 480 LVM_INT32 Dt0; 481 LVM_INT32 B2_Den; 482 LVM_INT32 B2_Num; 483 LVM_INT32 COS_T0; 484 LVM_INT16 coef; 485 LVM_INT32 factor; 486 LVM_INT16 t0; 487 LVM_INT16 i; 488 489 490 /* 491 * Get the filter definition 492 */ 493 LVM_UINT16 Frequency = pFilterParams->CenterFrequency; 494 LVM_UINT16 QFactor = pFilterParams->QFactor; 495 496 497 /* 498 * Calculating the intermediate values 499 */ 500 T0 = (LVM_INT32)Frequency * LVPSA_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 501 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 502 /* Force D = 1 : the function was originally used for a peaking filter. 503 The D parameter do not exist for a BandPass filter coefficients */ 504 505 /* 506 * Calculate the B2 coefficient 507 */ 508 Dt0 = D * (T0 >> 10); 509 B2_Den = (LVM_INT32)(((LVM_UINT32)QFactor << 19) + (LVM_UINT32)(Dt0 >> 2)); 510 B2_Num = (LVM_INT32)((LVM_UINT32)(Dt0 >> 3) - ((LVM_UINT32)QFactor << 18)); 511 B2 = (B2_Num / (B2_Den >> 16)) << 15; 512 513 /* 514 * Calculate the cosine by a polynomial expansion using the equation: 515 * 516 * Cos += coef(n) * t0^n For n = 0 to 6 517 */ 518 T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ 519 t0 = (LVM_INT16)(T0 >> 16); 520 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 521 COS_T0 = 0; /* Initialise the error to zero */ 522 for (i=1; i<7; i++) 523 { 524 coef = LVPSA_CosCoef[i]; /* Get the nth coefficient */ 525 COS_T0 += (factor * coef) >> 5; /* The nth partial sum */ 526 factor = (factor * t0) >> 15; /* Calculate t0^n */ 527 } 528 COS_T0 = COS_T0 << (LVPSA_CosCoef[0]+6); /* Correct the scaling */ 529 530 531 B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2) * cos(t0) */ 532 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) / 2 */ 533 534 /* 535 * Write coeff into the data structure 536 */ 537 pCoefficients->A0 = (LVM_INT16)(A0>>16); 538 pCoefficients->B1 = (LVM_INT16)(B1>>15); 539 pCoefficients->B2 = (LVM_INT16)(B2>>16); 540 541 542 return(LVPSA_OK); 543} 544 545/****************************************************************************************/ 546/* */ 547/* FUNCTION: LVPSA_BPDoublePrecCoefs */ 548/* */ 549/* DESCRIPTION: */ 550/* Calculate double precision coefficients for a band pass filter */ 551/* */ 552/* PARAMETERS: */ 553/* Fs Sampling frequency index */ 554/* pFilterParams Pointer to the filter definition */ 555/* pCoefficients Pointer to the coefficients */ 556/* */ 557/* RETURNS: */ 558/* LVPSA_OK Always succeeds */ 559/* */ 560/* NOTES: */ 561/* 1. The equations used are as follows: */ 562/* */ 563/* t0 = 2 * Pi * Fc / Fs */ 564/* */ 565/* b2 = -0.5 * (2Q - t0) / (2Q + t0) */ 566/* b1 = (0.5 - b2) * (1 - coserr(t0)) */ 567/* a0 = (0.5 + b2) / 2 */ 568/* */ 569/* Where: */ 570/* Fc is the centre frequency, DC to Fs/50 */ 571/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 572/* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */ 573/* */ 574/* 2. The double precision coefficients are only used when fc is less than fs/85, so */ 575/* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */ 576/* itself the difference from the value 1.0 is calculated, this can be done with */ 577/* lower precision maths. */ 578/* */ 579/* 3. The value of the B2 coefficient is only calculated as a single precision value, */ 580/* small errors in this value have a combined effect on the Q and Gain but not the */ 581/* the frequency of the filter. */ 582/* */ 583/* 4. This function is entirely based on the LVEQNB_DoublePrecCoefs function */ 584/* of the n bands equalizer (LVEQNB */ 585/* */ 586/****************************************************************************************/ 587LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 588 LVPSA_FilterParam_t *pFilterParams, 589 BP_C32_Coefs_t *pCoefficients) 590{ 591 592 extern LVM_INT16 LVPSA_TwoPiOnFsTable[]; 593 extern LVM_INT16 LVPSA_DPCosCoef[]; 594 595 /* 596 * Intermediate variables and temporary values 597 */ 598 LVM_INT32 T0; 599 LVM_INT16 D; 600 LVM_INT32 A0; 601 LVM_INT32 B1; 602 LVM_INT32 B2; 603 LVM_INT32 Dt0; 604 LVM_INT32 B2_Den; 605 LVM_INT32 B2_Num; 606 LVM_INT32 CosErr; 607 LVM_INT16 coef; 608 LVM_INT32 factor; 609 LVM_INT16 t0; 610 LVM_INT16 i; 611 612 /* 613 * Get the filter definition 614 */ 615 LVM_UINT16 Frequency = pFilterParams->CenterFrequency; 616 LVM_UINT16 QFactor = pFilterParams->QFactor; 617 618 619 /* 620 * Calculating the intermediate values 621 */ 622 T0 = (LVM_INT32)Frequency * LVPSA_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 623 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 624 /* Force D = 1 : the function was originally used for a peaking filter. 625 The D parameter do not exist for a BandPass filter coefficients */ 626 627 /* 628 * Calculate the B2 coefficient 629 */ 630 Dt0 = D * (T0 >> 10); 631 B2_Den = (LVM_INT32)(((LVM_UINT32)QFactor << 19) + (LVM_UINT32)(Dt0 >> 2)); 632 B2_Num = (LVM_INT32)((LVM_UINT32)(Dt0 >> 3) - ((LVM_UINT32)QFactor << 18)); 633 B2 = (B2_Num / (B2_Den >> 16)) << 15; 634 635 /* 636 * Calculate the cosine error by a polynomial expansion using the equation: 637 * 638 * CosErr += coef(n) * t0^n For n = 0 to 4 639 */ 640 T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ 641 t0 = (LVM_INT16)(T0 >> 16); 642 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 643 CosErr = 0; /* Initialise the error to zero */ 644 for (i=1; i<5; i++) 645 { 646 coef = LVPSA_DPCosCoef[i]; /* Get the nth coefficient */ 647 CosErr += (factor * coef) >> 5; /* The nth partial sum */ 648 factor = (factor * t0) >> 15; /* Calculate t0^n */ 649 } 650 CosErr = CosErr << (LVPSA_DPCosCoef[0]); /* Correct the scaling */ 651 652 /* 653 * Calculate the B1 and A0 coefficients 654 */ 655 B1 = (0x40000000 - B2); /* B1 = (0.5 - b2) */ 656 A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2) * coserr(t0) */ 657 B1 -= A0; /* B1 = (0.5 - b2) * (1 - coserr(t0)) */ 658 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) / 2 */ 659 660 /* 661 * Write coeff into the data structure 662 */ 663 pCoefficients->A0 = A0; 664 pCoefficients->B1 = B1; 665 pCoefficients->B2 = B2; 666 667 return(LVPSA_OK); 668} 669 670/************************************************************************************/ 671/* */ 672/* FUNCTION: LVPSA_ClearFilterHistory */ 673/* */ 674/* DESCRIPTION: */ 675/* Clears the filters' data history */ 676/* */ 677/* PARAMETERS: */ 678/* pInst Pointer to the instance */ 679/* */ 680/* RETURNS: */ 681/* LVPSA_OK Always succeeds */ 682/* */ 683/* NOTES: */ 684/* */ 685/************************************************************************************/ 686LVPSA_RETURN LVPSA_ClearFilterHistory(LVPSA_InstancePr_t *pInst) 687{ 688 LVM_INT8 *pTapAddress; 689 LVM_UINT32 i; 690 691 /* Band Pass filters taps */ 692 pTapAddress = (LVM_INT8 *)pInst->pBP_Taps; 693 for(i = 0; i < pInst->nBands * sizeof(Biquad_1I_Order2_Taps_t); i++) 694 { 695 pTapAddress[i] = 0; 696 } 697 698 /* Quasi-peak filters taps */ 699 pTapAddress = (LVM_INT8 *)pInst->pQPD_Taps; 700 for(i = 0; i < pInst->nBands * sizeof(QPD_Taps_t); i++) 701 { 702 pTapAddress[i] = 0; 703 } 704 705 return(LVPSA_OK); 706} 707 708