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_C16_Coefs_t *pCoefficients);
34
35 LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs,
36 LVPSA_FilterParam_t *pFilterParams,
37 BP_C32_Coefs_t *pCoefficients);
38
39 LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs,
40 LVPSA_FilterParam_t *pFilterParams,
41 BP_C32_Coefs_t *pCoefficients);
42
43 LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst,
44 LVPSA_ControlParams_t *pParams );
45
46 LVPSA_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 /************************************************************************************/
LVPSA_Control(pLVPSA_Handle_t hInstance,LVPSA_ControlParams_t * pNewParams)67 LVPSA_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 /************************************************************************************/
LVPSA_GetControlParams(pLVPSA_Handle_t hInstance,LVPSA_ControlParams_t * pParams)107 LVPSA_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 /************************************************************************************/
LVPSA_GetInitParams(pLVPSA_Handle_t hInstance,LVPSA_InitParams_t * pParams)139 LVPSA_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 /************************************************************************************/
LVPSA_ApplyNewSettings(LVPSA_InstancePr_t * pInst)176 LVPSA_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 /************************************************************************************/
LVPSA_SetBPFiltersType(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)267 LVPSA_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 /************************************************************************************/
LVPSA_SetBPFCoefficients(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)330 LVPSA_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 /************************************************************************************/
LVPSA_SetQPFCoefficients(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)407 LVPSA_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 /****************************************************************************************/
LVPSA_BPSinglePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_C16_Coefs_t * pCoefficients)463 LVPSA_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 /****************************************************************************************/
LVPSA_BPDoublePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_C32_Coefs_t * pCoefficients)587 LVPSA_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 /************************************************************************************/
LVPSA_ClearFilterHistory(LVPSA_InstancePr_t * pInst)686 LVPSA_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