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 #ifdef BUILD_FLOAT
32 LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs,
33 LVPSA_FilterParam_t *pFilterParams,
34 BP_FLOAT_Coefs_t *pCoefficients);
35
36 LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs,
37 LVPSA_FilterParam_t *pFilterParams,
38 BP_FLOAT_Coefs_t *pCoefficients);
39 #else
40 LVPSA_RETURN LVPSA_BPSinglePrecCoefs( LVM_UINT16 Fs,
41 LVPSA_FilterParam_t *pFilterParams,
42 BP_C16_Coefs_t *pCoefficients);
43
44 LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs,
45 LVPSA_FilterParam_t *pFilterParams,
46 BP_C32_Coefs_t *pCoefficients);
47
48 LVPSA_RETURN LVPSA_BPDoublePrecCoefs( LVM_UINT16 Fs,
49 LVPSA_FilterParam_t *pFilterParams,
50 BP_C32_Coefs_t *pCoefficients);
51 #endif
52 LVPSA_RETURN LVPSA_SetBPFCoefficients( LVPSA_InstancePr_t *pInst,
53 LVPSA_ControlParams_t *pParams );
54
55 LVPSA_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 /************************************************************************************/
LVPSA_Control(pLVPSA_Handle_t hInstance,LVPSA_ControlParams_t * pNewParams)76 LVPSA_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 /************************************************************************************/
LVPSA_GetControlParams(pLVPSA_Handle_t hInstance,LVPSA_ControlParams_t * pParams)116 LVPSA_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 /************************************************************************************/
LVPSA_GetInitParams(pLVPSA_Handle_t hInstance,LVPSA_InitParams_t * pParams)148 LVPSA_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 /************************************************************************************/
LVPSA_ApplyNewSettings(LVPSA_InstancePr_t * pInst)185 LVPSA_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 /************************************************************************************/
LVPSA_SetBPFiltersType(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)280 LVPSA_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 /************************************************************************************/
LVPSA_SetBPFCoefficients(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)346 LVPSA_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 /************************************************************************************/
LVPSA_SetQPFCoefficients(LVPSA_InstancePr_t * pInst,LVPSA_ControlParams_t * pParams)456 LVPSA_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
LVPSA_BPSinglePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_FLOAT_Coefs_t * pCoefficients)526 LVPSA_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
LVPSA_BPSinglePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_C16_Coefs_t * pCoefficients)606 LVPSA_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
LVPSA_BPDoublePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_FLOAT_Coefs_t * pCoefficients)731 LVPSA_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
LVPSA_BPDoublePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_C32_Coefs_t * pCoefficients)814 LVPSA_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 /************************************************************************************/
LVPSA_ClearFilterHistory(LVPSA_InstancePr_t * pInst)913 LVPSA_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