LVPSA_Control.c revision d7d013446a64c6de9f0f2dfe098a721b140e0b48
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 31#ifdef BUILD_FLOAT 32LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 33 LVPSA_FilterParam_t *pFilterParams, 34 BP_FLOAT_Coefs_t *pCoefficients); 35 36LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 37 LVPSA_FilterParam_t *pFilterParams, 38 BP_FLOAT_Coefs_t *pCoefficients); 39#else 40LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 41 LVPSA_FilterParam_t *pFilterParams, 42 BP_C16_Coefs_t *pCoefficients); 43 44LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 45 LVPSA_FilterParam_t *pFilterParams, 46 BP_C32_Coefs_t *pCoefficients); 47 48LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 49 LVPSA_FilterParam_t *pFilterParams, 50 BP_C32_Coefs_t *pCoefficients); 51#endif 52LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst, 53 LVPSA_ControlParams_t *pParams ); 54 55LVPSA_RETURN LVPSA_ClearFilterHistory( LVPSA_InstancePr_t *pInst); 56 57 58 59 60/************************************************************************************/ 61/* */ 62/* FUNCTION: LVPSA_Control */ 63/* */ 64/* DESCRIPTION: */ 65/* Give some new control parameters to the module. */ 66/* */ 67/* PARAMETERS: */ 68/* hInstance Pointer to the instance */ 69/* NewParams Structure that contains the new parameters */ 70/* */ 71/* RETURNS: */ 72/* LVPSA_OK Succeeds */ 73/* otherwise Error due to bad parameters */ 74/* */ 75/************************************************************************************/ 76LVPSA_RETURN LVPSA_Control ( pLVPSA_Handle_t hInstance, 77 LVPSA_ControlParams_t *pNewParams ) 78{ 79 80 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 81 82 if((hInstance == LVM_NULL) || (pNewParams == LVM_NULL)) 83 { 84 return(LVPSA_ERROR_NULLADDRESS); 85 } 86 if(pNewParams->Fs >= LVPSA_NR_SUPPORTED_RATE) 87 { 88 return(LVPSA_ERROR_INVALIDPARAM); 89 } 90 if(pNewParams->LevelDetectionSpeed >= LVPSA_NR_SUPPORTED_SPEED) 91 { 92 return(LVPSA_ERROR_INVALIDPARAM); 93 } 94 95 pLVPSA_Inst->NewParams = *pNewParams; 96 pLVPSA_Inst->bControlPending = LVM_TRUE; 97 98 return(LVPSA_OK); 99} 100 101/************************************************************************************/ 102/* */ 103/* FUNCTION: LVPSA_GetControlParams */ 104/* */ 105/* DESCRIPTION: */ 106/* Get the current control parameters of the module */ 107/* */ 108/* PARAMETERS: */ 109/* hInstance Pointer to the instance */ 110/* pParams Pointer to an empty control structure */ 111/* RETURNS: */ 112/* LVPSA_OK Succeeds */ 113/* otherwise Error due to bad parameters */ 114/* */ 115/************************************************************************************/ 116LVPSA_RETURN LVPSA_GetControlParams ( pLVPSA_Handle_t hInstance, 117 LVPSA_ControlParams_t *pParams ) 118{ 119 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 120 121 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 122 { 123 return(LVPSA_ERROR_NULLADDRESS); 124 } 125 126 pParams->Fs = pLVPSA_Inst->CurrentParams.Fs; 127 pParams->LevelDetectionSpeed = pLVPSA_Inst->CurrentParams.LevelDetectionSpeed; 128 129 return(LVPSA_OK); 130} 131 132 133/************************************************************************************/ 134/* */ 135/* FUNCTION: LVPSA_GetInitParams */ 136/* */ 137/* DESCRIPTION: */ 138/* Get the initialization parameters of the module */ 139/* */ 140/* PARAMETERS: */ 141/* hInstance Pointer to the instance */ 142/* pParams Pointer to an empty control structure */ 143/* RETURNS: */ 144/* LVPSA_OK Succeeds */ 145/* otherwise Error due to bad parameters */ 146/* */ 147/************************************************************************************/ 148LVPSA_RETURN LVPSA_GetInitParams ( pLVPSA_Handle_t hInstance, 149 LVPSA_InitParams_t *pParams ) 150{ 151 LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance; 152 153 if((hInstance == LVM_NULL) || (pParams == LVM_NULL)) 154 { 155 return(LVPSA_ERROR_NULLADDRESS); 156 } 157 158 pParams->SpectralDataBufferDuration = pLVPSA_Inst->SpectralDataBufferDuration; 159 pParams->MaxInputBlockSize = pLVPSA_Inst->MaxInputBlockSize; 160 pParams->nBands = pLVPSA_Inst->nBands; 161 pParams->pFiltersParams = pLVPSA_Inst->pFiltersParams; 162 163 return(LVPSA_OK); 164} 165 166 167/************************************************************************************/ 168/* */ 169/* FUNCTION: LVPSA_ApplyNewSettings */ 170/* */ 171/* DESCRIPTION: */ 172/* Reinitialize some parameters and changes filters' coefficients if */ 173/* some control parameters have changed. */ 174/* */ 175/* PARAMETERS: */ 176/* pInst Pointer to the instance */ 177/* */ 178/* RETURNS: */ 179/* LVPSA_OK Succeeds */ 180/* otherwise Error due to bad parameters */ 181/* */ 182/* NOTES: */ 183/* */ 184/************************************************************************************/ 185LVPSA_RETURN LVPSA_ApplyNewSettings (LVPSA_InstancePr_t *pInst) 186{ 187 LVM_UINT16 ii; 188 LVM_UINT16 Freq; 189 LVPSA_ControlParams_t Params; 190 extern LVM_INT16 LVPSA_nSamplesBufferUpdate[]; 191#ifndef HIGHER_FS 192 extern LVM_UINT16 LVPSA_SampleRateTab[]; 193#else 194 extern LVM_UINT32 LVPSA_SampleRateTab[]; 195#endif 196 extern LVM_UINT16 LVPSA_DownSamplingFactor[]; 197 198 199 if(pInst == 0) 200 { 201 return(LVPSA_ERROR_NULLADDRESS); 202 } 203 204 Params = pInst->NewParams; 205 206 /* Modifies filters types and coefficients, clear the taps and 207 re-initializes parameters if sample frequency has changed */ 208 if(Params.Fs != pInst->CurrentParams.Fs) 209 { 210 pInst->CurrentParams.Fs = Params.Fs; 211 212 /* Initialize the center freqeuncies as a function of the sample rate */ 213 Freq = (LVM_UINT16) ((LVPSA_SampleRateTab[pInst->CurrentParams.Fs]>>1) / (pInst->nBands + 1)); 214 for(ii = pInst->nBands; ii > 0; ii--) 215 { 216 pInst->pFiltersParams[ii-1].CenterFrequency = (LVM_UINT16) (Freq * ii); 217 } 218 219 /* Count the number of relevant filters. If the center frequency of the filter is 220 bigger than the nyquist frequency, then the filter is not relevant and doesn't 221 need to be used */ 222 for(ii = pInst->nBands; ii > 0; ii--) 223 { 224 if(pInst->pFiltersParams[ii-1].CenterFrequency < (LVPSA_SampleRateTab[pInst->CurrentParams.Fs]>>1)) 225 { 226 pInst->nRelevantFilters = ii; 227 break; 228 } 229 } 230 LVPSA_SetBPFiltersType(pInst, &Params); 231 LVPSA_SetBPFCoefficients(pInst, &Params); 232 LVPSA_SetQPFCoefficients(pInst, &Params); 233 LVPSA_ClearFilterHistory(pInst); 234 pInst->nSamplesBufferUpdate = (LVM_UINT16)LVPSA_nSamplesBufferUpdate[Params.Fs]; 235 pInst->BufferUpdateSamplesCount = 0; 236 pInst->DownSamplingFactor = LVPSA_DownSamplingFactor[Params.Fs]; 237 pInst->DownSamplingCount = 0; 238 for(ii = 0; ii < (pInst->nBands * pInst->SpectralDataBufferLength); ii++) 239 { 240 pInst->pSpectralDataBufferStart[ii] = 0; 241 } 242 for(ii = 0; ii < pInst->nBands; ii++) 243 { 244 pInst->pPreviousPeaks[ii] = 0; 245 } 246 } 247 else 248 { 249 if(Params.LevelDetectionSpeed != pInst->CurrentParams.LevelDetectionSpeed) 250 { 251 LVPSA_SetQPFCoefficients(pInst, &Params); 252 } 253 } 254 255 pInst->CurrentParams = pInst->NewParams; 256 257 return (LVPSA_OK); 258} 259/************************************************************************************/ 260/* */ 261/* FUNCTION: LVPSA_SetBPFiltersType */ 262/* */ 263/* DESCRIPTION: */ 264/* Sets the filter type based on the BPFilterType. */ 265/* */ 266/* PARAMETERS: */ 267/* pInst Pointer to the instance */ 268/* pParams Poniter to conrol parameters */ 269/* */ 270/* RETURNS: */ 271/* LVPSA_OK Always succeeds */ 272/* */ 273/* NOTES: */ 274/* 1. To select the biquad type the follow rules are applied: */ 275/* Double precision if (fc <= fs/110) */ 276/* Double precision if (fs/110 < fc < fs/85) & (Q>3) */ 277/* Single precision otherwise */ 278/* */ 279/************************************************************************************/ 280LVPSA_RETURN LVPSA_SetBPFiltersType ( LVPSA_InstancePr_t *pInst, 281 LVPSA_ControlParams_t *pParams ) 282{ 283#ifndef HIGHER_FS 284 extern LVM_UINT16 LVPSA_SampleRateTab[]; /* Sample rate table */ 285#else 286 extern LVM_UINT32 LVPSA_SampleRateTab[]; /* Sample rate table */ 287#endif 288 LVM_UINT16 ii; /* Filter band index */ 289 LVM_UINT32 fs = (LVM_UINT32)LVPSA_SampleRateTab[(LVM_UINT16)pParams->Fs]; /* Sample rate */ 290 LVM_UINT32 fc; /* Filter centre frequency */ 291 LVM_INT16 QFactor; /* Filter Q factor */ 292 293 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 294 { 295 /* 296 * Get the filter settings 297 */ 298 fc = (LVM_UINT32)pInst->pFiltersParams[ii].CenterFrequency; /* Get the band centre frequency */ 299 QFactor =(LVM_INT16) pInst->pFiltersParams[ii].QFactor; /* Get the band Q factor */ 300 301 302 /* 303 * For each filter set the type of biquad required 304 */ 305 pInst->pBPFiltersPrecision[ii] = LVPSA_SimplePrecisionFilter; /* Default to single precision */ 306 if ((LOW_FREQ * fs) >= (fc << 15)) 307 { 308 /* 309 * fc <= fs/110 310 */ 311 pInst->pBPFiltersPrecision[ii] = LVPSA_DoublePrecisionFilter; 312 } 313 else 314 { 315 if (((LOW_FREQ * fs) < (fc << 15)) && ((fc << 15) < (HIGH_FREQ * fs)) && (QFactor > 300)) 316 { 317 /* 318 * (fs/110 < fc < fs/85) & (Q>3) 319 */ 320 pInst->pBPFiltersPrecision[ii] = LVPSA_DoublePrecisionFilter; 321 } 322 } 323 } 324 325 return(LVPSA_OK); 326} 327 328/************************************************************************************/ 329/* */ 330/* FUNCTION: LVPSA_SetBPFCoefficients */ 331/* */ 332/* DESCRIPTION: */ 333/* Sets the band pass filter coefficients. This uses the type to select */ 334/* single or double precision coefficients. */ 335/* */ 336/* PARAMETERS: */ 337/* pInst Pointer to the instance */ 338/* Params Initialisation parameters */ 339/* */ 340/* RETURNS: */ 341/* LVPSA_OK Always succeeds */ 342/* */ 343/* NOTES: */ 344/* */ 345/************************************************************************************/ 346LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst, 347 LVPSA_ControlParams_t *pParams) 348{ 349 350 LVM_UINT16 ii; 351 352 /* 353 * Set the coefficients for each band by the init function 354 */ 355 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 356 { 357 switch (pInst->pBPFiltersPrecision[ii]) 358 { 359 case LVPSA_DoublePrecisionFilter: 360 { 361#ifndef BUILD_FLOAT 362 BP_C32_Coefs_t Coefficients; 363 364 /* 365 * Calculate the double precision coefficients 366 */ 367 LVPSA_BPDoublePrecCoefs((LVM_UINT16)pParams->Fs, 368 &pInst->pFiltersParams[ii], 369 &Coefficients); 370 /* 371 * Set the coefficients 372 */ 373 BP_1I_D16F32Cll_TRC_WRA_01_Init ( &pInst->pBP_Instances[ii], 374 &pInst->pBP_Taps[ii], 375 &Coefficients); 376#else 377 BP_FLOAT_Coefs_t Coefficients; 378 /* 379 * Calculate the double precision coefficients 380 */ 381 LVPSA_BPDoublePrecCoefs((LVM_UINT16)pParams->Fs, 382 &pInst->pFiltersParams[ii], 383 &Coefficients); 384 /* 385 * Set the coefficients 386 */ 387 BP_1I_D16F32Cll_TRC_WRA_01_Init ( &pInst->pBP_Instances[ii], 388 &pInst->pBP_Taps[ii], 389 &Coefficients); 390#endif 391 break; 392 } 393 394 case LVPSA_SimplePrecisionFilter: 395 { 396#ifndef BUILD_FLOAT 397 BP_C16_Coefs_t Coefficients; 398 399 /* 400 * Calculate the single precision coefficients 401 */ 402 LVPSA_BPSinglePrecCoefs((LVM_UINT16)pParams->Fs, 403 &pInst->pFiltersParams[ii], 404 &Coefficients); 405 406 /* 407 * Set the coefficients 408 */ 409 BP_1I_D16F16Css_TRC_WRA_01_Init (&pInst->pBP_Instances[ii], 410 &pInst->pBP_Taps[ii], 411 &Coefficients); 412#else 413 BP_FLOAT_Coefs_t Coefficients; 414 415 /* 416 * Calculate the single precision coefficients 417 */ 418 LVPSA_BPSinglePrecCoefs((LVM_UINT16)pParams->Fs, 419 &pInst->pFiltersParams[ii], 420 &Coefficients); 421 422 /* 423 * Set the coefficients 424 */ 425 BP_1I_D16F16Css_TRC_WRA_01_Init (&pInst->pBP_Instances[ii], 426 &pInst->pBP_Taps[ii], 427 &Coefficients); 428#endif 429 break; 430 } 431 } 432 } 433 434 return(LVPSA_OK); 435} 436 437 438/************************************************************************************/ 439/* */ 440/* FUNCTION: LVPSA_SetQPFCoefficients */ 441/* */ 442/* DESCRIPTION: */ 443/* Sets the quasi peak filters coefficients. This uses the chosen */ 444/* LevelDetectionSpeed from the control parameters. */ 445/* */ 446/* PARAMETERS: */ 447/* pInst Pointer to the instance */ 448/* Params Control parameters */ 449/* */ 450/* RETURNS: */ 451/* LVPSA_OK Always succeeds */ 452/* */ 453/* NOTES: */ 454/* */ 455/************************************************************************************/ 456LVPSA_RETURN LVPSA_SetQPFCoefficients( LVPSA_InstancePr_t *pInst, 457 LVPSA_ControlParams_t *pParams ) 458{ 459 LVM_UINT16 ii; 460 LVM_Fs_en Fs = pParams->Fs; 461#ifndef BUILD_FLOAT 462 QPD_C32_Coefs *pCoefficients; 463 extern QPD_C32_Coefs LVPSA_QPD_Coefs[]; 464 465 pCoefficients = &LVPSA_QPD_Coefs[(pParams->LevelDetectionSpeed * LVPSA_NR_SUPPORTED_RATE) + Fs]; 466#else 467 QPD_FLOAT_Coefs *pCoefficients; 468 extern QPD_FLOAT_Coefs LVPSA_QPD_Float_Coefs[]; 469 470 pCoefficients = &LVPSA_QPD_Float_Coefs[(pParams->LevelDetectionSpeed * \ 471 LVPSA_NR_SUPPORTED_RATE) + Fs]; 472#endif 473 474 475 for (ii = 0; ii < pInst->nRelevantFilters; ii++) 476 { 477#ifndef BUILD_FLOAT 478 LVPSA_QPD_Init (&pInst->pQPD_States[ii], 479 &pInst->pQPD_Taps[ii], 480 pCoefficients ); 481#else 482 LVPSA_QPD_Init_Float (&pInst->pQPD_States[ii], 483 &pInst->pQPD_Taps[ii], 484 pCoefficients ); 485#endif 486 } 487 488 return(LVPSA_OK); 489 490} 491 492/****************************************************************************************/ 493/* */ 494/* FUNCTION: LVPSA_BPSinglePrecCoefs */ 495/* */ 496/* DESCRIPTION: */ 497/* Calculate single precision coefficients for a band pass filter */ 498/* */ 499/* PARAMETERS: */ 500/* Fs Sampling frequency index */ 501/* pFilterParams Pointer to the filter definition */ 502/* pCoefficients Pointer to the coefficients */ 503/* */ 504/* RETURNS: */ 505/* LVPSA_OK Always succeeds */ 506/* */ 507/* NOTES: */ 508/* 1. The equations used are as follows: */ 509/* */ 510/* t0 = 2 * Pi * Fc / Fs */ 511/* */ 512/* b2 = -0.5 * (2Q - t0) / (2Q + t0) */ 513/* b1 = (0.5 - b2) * cos(t0) */ 514/* a0 = (0.5 + b2) / 2 */ 515/* */ 516/* Where: */ 517/* Fc is the centre frequency, DC to Nyquist */ 518/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 519/* Q is the Q factor, 0.25 to 12 */ 520/* */ 521/* 2. This function is entirely based on the LVEQNB_SinglePrecCoefs function */ 522/* of the n bands equalizer (LVEQNB */ 523/* */ 524/****************************************************************************************/ 525#ifdef BUILD_FLOAT 526LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 527 LVPSA_FilterParam_t *pFilterParams, 528 BP_FLOAT_Coefs_t *pCoefficients) 529{ 530 531 extern LVM_FLOAT LVPSA_Float_TwoPiOnFsTable[]; 532 extern LVM_FLOAT LVPSA_Float_CosCoef[]; 533 534 535 /* 536 * Intermediate variables and temporary values 537 */ 538 LVM_FLOAT T0; 539 LVM_FLOAT D; 540 LVM_FLOAT A0; 541 LVM_FLOAT B1; 542 LVM_FLOAT B2; 543 LVM_FLOAT Dt0; 544 LVM_FLOAT B2_Den; 545 LVM_FLOAT B2_Num; 546 LVM_FLOAT COS_T0; 547 LVM_FLOAT coef; 548 LVM_FLOAT factor; 549 LVM_FLOAT t0; 550 LVM_INT16 i; 551 552 553 /* 554 * Get the filter definition 555 */ 556 LVM_FLOAT Frequency = (LVM_FLOAT)(pFilterParams->CenterFrequency); 557 LVM_FLOAT QFactor = ((LVM_FLOAT)(pFilterParams->QFactor)) / 100; 558 559 /* 560 * Calculating the intermediate values 561 */ 562 T0 = Frequency * LVPSA_Float_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 563 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 564 /* Force D = 1 : the function was originally used for a peaking filter. 565 The D parameter do not exist for a BandPass filter coefficients */ 566 567 /* 568 * Calculate the B2 coefficient 569 */ 570 Dt0 = T0 / 2048 ; 571 B2_Den = QFactor + Dt0; 572 B2_Num = Dt0 - QFactor; 573 B2 = B2_Num / (2 * B2_Den); 574 575 /* 576 * Calculate the cosine by a polynomial expansion using the equation: 577 * 578 * Cos += coef(n) * t0^n For n = 0 to 6 579 */ 580 T0 = (T0 / 2048) * 0.63658558f; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ 581 t0 = T0 ; 582 factor = 1.0f; /* Initialise to 1.0 for the a0 coefficient */ 583 COS_T0 = 0.0f; /* Initialise the error to zero */ 584 for (i = 1; i < 7; i++) 585 { 586 coef = LVPSA_Float_CosCoef[i]; /* Get the nth coefficient */ 587 COS_T0 += (factor * coef); /* The nth partial sum */ 588 factor = (factor * t0) ; /* Calculate t0^n */ 589 } 590 COS_T0 = COS_T0 * 8; /*LVPSA_CosCoef_float[0]*/ /* Correct the scaling */ 591 592 593 B1 = ((LVM_FLOAT)0.5 - B2) * (COS_T0); /* B1 = (0.5 - b2) * cos(t0) */ 594 A0 = ((LVM_FLOAT)0.5 + B2) / 2; /* A0 = (0.5 + b2) / 2 */ 595 596 /* 597 * Write coeff into the data structure 598 */ 599 pCoefficients->A0 = A0 * 2; 600 pCoefficients->B1 = B1 * 2; 601 pCoefficients->B2 = B2 * 2; 602 603 return(LVPSA_OK); 604} 605#else 606LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs, 607 LVPSA_FilterParam_t *pFilterParams, 608 BP_C16_Coefs_t *pCoefficients) 609{ 610 611 extern LVM_INT16 LVPSA_TwoPiOnFsTable[]; 612 extern LVM_INT16 LVPSA_CosCoef[]; 613 614 615 /* 616 * Intermediate variables and temporary values 617 */ 618 LVM_INT32 T0; 619 LVM_INT16 D; 620 LVM_INT32 A0; 621 LVM_INT32 B1; 622 LVM_INT32 B2; 623 LVM_INT32 Dt0; 624 LVM_INT32 B2_Den; 625 LVM_INT32 B2_Num; 626 LVM_INT32 COS_T0; 627 LVM_INT16 coef; 628 LVM_INT32 factor; 629 LVM_INT16 t0; 630 LVM_INT16 i; 631 632 633 /* 634 * Get the filter definition 635 */ 636 LVM_UINT16 Frequency = pFilterParams->CenterFrequency; 637 LVM_UINT16 QFactor = pFilterParams->QFactor; 638 639 640 /* 641 * Calculating the intermediate values 642 */ 643 T0 = (LVM_INT32)Frequency * LVPSA_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 644 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 645 /* Force D = 1 : the function was originally used for a peaking filter. 646 The D parameter do not exist for a BandPass filter coefficients */ 647 648 /* 649 * Calculate the B2 coefficient 650 */ 651 Dt0 = D * (T0 >> 10); 652 B2_Den = (LVM_INT32)(((LVM_UINT32)QFactor << 19) + (LVM_UINT32)(Dt0 >> 2)); 653 B2_Num = (LVM_INT32)((LVM_UINT32)(Dt0 >> 3) - ((LVM_UINT32)QFactor << 18)); 654 B2 = (B2_Num / (B2_Den >> 16)) << 15; 655 656 /* 657 * Calculate the cosine by a polynomial expansion using the equation: 658 * 659 * Cos += coef(n) * t0^n For n = 0 to 6 660 */ 661 T0 = (T0 >> 10) * 20859; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */ 662 t0 = (LVM_INT16)(T0 >> 16); 663 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 664 COS_T0 = 0; /* Initialise the error to zero */ 665 for (i=1; i<7; i++) 666 { 667 coef = LVPSA_CosCoef[i]; /* Get the nth coefficient */ 668 COS_T0 += (factor * coef) >> 5; /* The nth partial sum */ 669 factor = (factor * t0) >> 15; /* Calculate t0^n */ 670 } 671 COS_T0 = COS_T0 << (LVPSA_CosCoef[0]+6); /* Correct the scaling */ 672 673 674 B1 = ((0x40000000 - B2) >> 16) * (COS_T0 >> 16); /* B1 = (0.5 - b2) * cos(t0) */ 675 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) / 2 */ 676 677 /* 678 * Write coeff into the data structure 679 */ 680 pCoefficients->A0 = (LVM_INT16)(A0>>16); 681 pCoefficients->B1 = (LVM_INT16)(B1>>15); 682 pCoefficients->B2 = (LVM_INT16)(B2>>16); 683 684 685 return(LVPSA_OK); 686} 687#endif 688/****************************************************************************************/ 689/* */ 690/* FUNCTION: LVPSA_BPDoublePrecCoefs */ 691/* */ 692/* DESCRIPTION: */ 693/* Calculate double precision coefficients for a band pass filter */ 694/* */ 695/* PARAMETERS: */ 696/* Fs Sampling frequency index */ 697/* pFilterParams Pointer to the filter definition */ 698/* pCoefficients Pointer to the coefficients */ 699/* */ 700/* RETURNS: */ 701/* LVPSA_OK Always succeeds */ 702/* */ 703/* NOTES: */ 704/* 1. The equations used are as follows: */ 705/* */ 706/* t0 = 2 * Pi * Fc / Fs */ 707/* */ 708/* b2 = -0.5 * (2Q - t0) / (2Q + t0) */ 709/* b1 = (0.5 - b2) * (1 - coserr(t0)) */ 710/* a0 = (0.5 + b2) / 2 */ 711/* */ 712/* Where: */ 713/* Fc is the centre frequency, DC to Fs/50 */ 714/* Fs is the sample frequency, 8000 to 48000 in descrete steps */ 715/* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */ 716/* */ 717/* 2. The double precision coefficients are only used when fc is less than fs/85, so */ 718/* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */ 719/* itself the difference from the value 1.0 is calculated, this can be done with */ 720/* lower precision maths. */ 721/* */ 722/* 3. The value of the B2 coefficient is only calculated as a single precision value, */ 723/* small errors in this value have a combined effect on the Q and Gain but not the */ 724/* the frequency of the filter. */ 725/* */ 726/* 4. This function is entirely based on the LVEQNB_DoublePrecCoefs function */ 727/* of the n bands equalizer (LVEQNB */ 728/* */ 729/****************************************************************************************/ 730#ifdef BUILD_FLOAT 731LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 732 LVPSA_FilterParam_t *pFilterParams, 733 BP_FLOAT_Coefs_t *pCoefficients) 734{ 735 736 extern LVM_FLOAT LVPSA_Float_TwoPiOnFsTable[]; 737 extern LVM_FLOAT LVPSA_Float_DPCosCoef[]; 738 739 /* 740 * Intermediate variables and temporary values 741 */ 742 LVM_FLOAT T0; 743 LVM_FLOAT D; 744 LVM_FLOAT A0; 745 LVM_FLOAT B1; 746 LVM_FLOAT B2; 747 LVM_FLOAT Dt0; 748 LVM_FLOAT B2_Den; 749 LVM_FLOAT B2_Num; 750 LVM_FLOAT CosErr; 751 LVM_FLOAT coef; 752 LVM_FLOAT factor; 753 LVM_FLOAT t0; 754 LVM_INT16 i; 755 756 /* 757 * Get the filter definition 758 */ 759 LVM_FLOAT Frequency = (LVM_FLOAT)(pFilterParams->CenterFrequency); 760 LVM_FLOAT QFactor = ((LVM_FLOAT)(pFilterParams->QFactor)) / 100; 761 762 763 /* 764 * Calculating the intermediate values 765 */ 766 T0 = Frequency * LVPSA_Float_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 767 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 768 /* Force D = 1 : the function was originally used for a peaking filter. 769 The D parameter do not exist for a BandPass filter coefficients */ 770 771 /* 772 * Calculate the B2 coefficient 773 */ 774 Dt0 = T0 / 2048 ; 775 B2_Den = QFactor + Dt0; 776 B2_Num = Dt0 - QFactor; 777 B2 = B2_Num / (2 * B2_Den); 778 779 /* 780 * Calculate the cosine error by a polynomial expansion using the equation: 781 * 782 * CosErr += coef(n) * t0^n For n = 0 to 4 783 */ 784 T0 = T0 * 0.994750f; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ 785 t0 = T0; 786 factor = 1.0f; /* Initialise to 1.0 for the a0 coefficient */ 787 CosErr = 0.0f; /* Initialise the error to zero */ 788 for (i = 1; i < 5; i++) 789 { 790 coef = LVPSA_Float_DPCosCoef[i]; /* Get the nth coefficient */ 791 CosErr += factor * coef; /* The nth partial sum */ 792 factor = factor * t0; /* Calculate t0^n */ 793 } 794 CosErr = CosErr * 2; /* Correct the scaling */ 795 796 /* 797 * Calculate the B1 and A0 coefficients 798 */ 799 B1 = ((LVM_FLOAT)0.5 - B2); /* B1 = (0.5 - b2) */ 800 A0 = B1 * CosErr ; /* Temporary storage for (0.5 - b2) * coserr(t0) */ 801 B1 -= A0; /* B1 = (0.5 - b2) * (1 - coserr(t0)) */ 802 A0 = ((LVM_FLOAT)0.5 + B2) / 2; /* A0 = (0.5 + b2) / 2 */ 803 804 /* 805 * Write coeff into the data structure 806 */ 807 pCoefficients->A0 = A0; 808 pCoefficients->B1 = B1; 809 pCoefficients->B2 = B2; 810 811 return(LVPSA_OK); 812} 813#else 814LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs, 815 LVPSA_FilterParam_t *pFilterParams, 816 BP_C32_Coefs_t *pCoefficients) 817{ 818 819 extern LVM_INT16 LVPSA_TwoPiOnFsTable[]; 820 extern LVM_INT16 LVPSA_DPCosCoef[]; 821 822 /* 823 * Intermediate variables and temporary values 824 */ 825 LVM_INT32 T0; 826 LVM_INT16 D; 827 LVM_INT32 A0; 828 LVM_INT32 B1; 829 LVM_INT32 B2; 830 LVM_INT32 Dt0; 831 LVM_INT32 B2_Den; 832 LVM_INT32 B2_Num; 833 LVM_INT32 CosErr; 834 LVM_INT16 coef; 835 LVM_INT32 factor; 836 LVM_INT16 t0; 837 LVM_INT16 i; 838 839 /* 840 * Get the filter definition 841 */ 842 LVM_UINT16 Frequency = pFilterParams->CenterFrequency; 843 LVM_UINT16 QFactor = pFilterParams->QFactor; 844 845 846 /* 847 * Calculating the intermediate values 848 */ 849 T0 = (LVM_INT32)Frequency * LVPSA_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */ 850 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */ 851 /* Force D = 1 : the function was originally used for a peaking filter. 852 The D parameter do not exist for a BandPass filter coefficients */ 853 854 /* 855 * Calculate the B2 coefficient 856 */ 857 Dt0 = D * (T0 >> 10); 858 B2_Den = (LVM_INT32)(((LVM_UINT32)QFactor << 19) + (LVM_UINT32)(Dt0 >> 2)); 859 B2_Num = (LVM_INT32)((LVM_UINT32)(Dt0 >> 3) - ((LVM_UINT32)QFactor << 18)); 860 B2 = (B2_Num / (B2_Den >> 16)) << 15; 861 862 /* 863 * Calculate the cosine error by a polynomial expansion using the equation: 864 * 865 * CosErr += coef(n) * t0^n For n = 0 to 4 866 */ 867 T0 = (T0 >> 6) * 0x7f53; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */ 868 t0 = (LVM_INT16)(T0 >> 16); 869 factor = 0x7fff; /* Initialise to 1.0 for the a0 coefficient */ 870 CosErr = 0; /* Initialise the error to zero */ 871 for (i=1; i<5; i++) 872 { 873 coef = LVPSA_DPCosCoef[i]; /* Get the nth coefficient */ 874 CosErr += (factor * coef) >> 5; /* The nth partial sum */ 875 factor = (factor * t0) >> 15; /* Calculate t0^n */ 876 } 877 CosErr = CosErr << (LVPSA_DPCosCoef[0]); /* Correct the scaling */ 878 879 /* 880 * Calculate the B1 and A0 coefficients 881 */ 882 B1 = (0x40000000 - B2); /* B1 = (0.5 - b2) */ 883 A0 = ((B1 >> 16) * (CosErr >> 10)) >> 6; /* Temporary storage for (0.5 - b2) * coserr(t0) */ 884 B1 -= A0; /* B1 = (0.5 - b2) * (1 - coserr(t0)) */ 885 A0 = (0x40000000 + B2) >> 1; /* A0 = (0.5 + b2) / 2 */ 886 887 /* 888 * Write coeff into the data structure 889 */ 890 pCoefficients->A0 = A0; 891 pCoefficients->B1 = B1; 892 pCoefficients->B2 = B2; 893 894 return(LVPSA_OK); 895} 896#endif 897/************************************************************************************/ 898/* */ 899/* FUNCTION: LVPSA_ClearFilterHistory */ 900/* */ 901/* DESCRIPTION: */ 902/* Clears the filters' data history */ 903/* */ 904/* PARAMETERS: */ 905/* pInst Pointer to the instance */ 906/* */ 907/* RETURNS: */ 908/* LVPSA_OK Always succeeds */ 909/* */ 910/* NOTES: */ 911/* */ 912/************************************************************************************/ 913LVPSA_RETURN LVPSA_ClearFilterHistory(LVPSA_InstancePr_t *pInst) 914{ 915 LVM_INT8 *pTapAddress; 916 LVM_UINT32 i; 917 918 /* Band Pass filters taps */ 919 pTapAddress = (LVM_INT8 *)pInst->pBP_Taps; 920#ifdef BUILD_FLOAT 921 for(i = 0; i < pInst->nBands * sizeof(Biquad_1I_Order2_FLOAT_Taps_t); i++) 922 { 923 pTapAddress[i] = 0; 924 } 925#else 926 for(i = 0; i < pInst->nBands * sizeof(Biquad_1I_Order2_Taps_t); i++) 927 { 928 pTapAddress[i] = 0; 929 } 930#endif 931 /* Quasi-peak filters taps */ 932 pTapAddress = (LVM_INT8 *)pInst->pQPD_Taps; 933 for(i = 0; i < pInst->nBands * sizeof(QPD_Taps_t); i++) 934 { 935 pTapAddress[i] = 0; 936 } 937 938 return(LVPSA_OK); 939} 940 941