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