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