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 D;
425 LVM_FLOAT A0;
426 LVM_FLOAT B1;
427 LVM_FLOAT B2;
428 LVM_FLOAT Dt0;
429 LVM_FLOAT B2_Den;
430 LVM_FLOAT B2_Num;
431 LVM_FLOAT COS_T0;
432 LVM_FLOAT coef;
433 LVM_FLOAT factor;
434 LVM_FLOAT t0;
435 LVM_INT16 i;
436
437 /*
438 * Get the filter definition
439 */
440 LVM_FLOAT Frequency = (LVM_FLOAT)(pFilterParams->CenterFrequency);
441 LVM_FLOAT QFactor = ((LVM_FLOAT)(pFilterParams->QFactor)) / 100;
442
443 /*
444 * Calculating the intermediate values
445 */
446 T0 = Frequency * LVPSA_Float_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */
447 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */
448 /* Force D = 1 : the function was originally used for a peaking filter.
449 The D parameter do not exist for a BandPass filter coefficients */
450
451 /*
452 * Calculate the B2 coefficient
453 */
454 Dt0 = T0 / 2048;
455 B2_Den = QFactor + Dt0;
456 B2_Num = Dt0 - QFactor;
457 B2 = B2_Num / (2 * B2_Den);
458
459 /*
460 * Calculate the cosine by a polynomial expansion using the equation:
461 *
462 * Cos += coef(n) * t0^n For n = 0 to 6
463 */
464 T0 = (T0 / 2048) * 0.63658558f; /* Scale to 1.0 in 16-bit for range 0 to fs/2 */
465 t0 = T0;
466 factor = 1.0f; /* Initialise to 1.0 for the a0 coefficient */
467 COS_T0 = 0.0f; /* Initialise the error to zero */
468 for (i = 1; i < 7; i++) {
469 coef = LVPSA_Float_CosCoef[i]; /* Get the nth coefficient */
470 COS_T0 += (factor * coef); /* The nth partial sum */
471 factor = (factor * t0); /* Calculate t0^n */
472 }
473 COS_T0 = COS_T0 * 8; /*LVPSA_CosCoef_float[0]*/ /* Correct the scaling */
474
475 B1 = ((LVM_FLOAT)0.5 - B2) * (COS_T0); /* B1 = (0.5 - b2) * cos(t0) */
476 A0 = ((LVM_FLOAT)0.5 + B2) / 2; /* A0 = (0.5 + b2) / 2 */
477
478 /*
479 * Write coeff into the data structure
480 */
481 pCoefficients->A0 = A0 * 2;
482 pCoefficients->B1 = B1 * 2;
483 pCoefficients->B2 = B2 * 2;
484
485 return (LVPSA_OK);
486 }
487 /****************************************************************************************/
488 /* */
489 /* FUNCTION: LVPSA_BPDoublePrecCoefs */
490 /* */
491 /* DESCRIPTION: */
492 /* Calculate double precision coefficients for a band pass filter */
493 /* */
494 /* PARAMETERS: */
495 /* Fs Sampling frequency index */
496 /* pFilterParams Pointer to the filter definition */
497 /* pCoefficients Pointer to the coefficients */
498 /* */
499 /* RETURNS: */
500 /* LVPSA_OK Always succeeds */
501 /* */
502 /* NOTES: */
503 /* 1. The equations used are as follows: */
504 /* */
505 /* t0 = 2 * Pi * Fc / Fs */
506 /* */
507 /* b2 = -0.5 * (2Q - t0) / (2Q + t0) */
508 /* b1 = (0.5 - b2) * (1 - coserr(t0)) */
509 /* a0 = (0.5 + b2) / 2 */
510 /* */
511 /* Where: */
512 /* Fc is the centre frequency, DC to Fs/50 */
513 /* Fs is the sample frequency, 8000 to 48000 in descrete steps */
514 /* Q is the Q factor, 0.25 to 12 (represented by 25 to 1200) */
515 /* */
516 /* 2. The double precision coefficients are only used when fc is less than fs/85, so */
517 /* the cosine of t0 is always close to 1.0. Instead of calculating the cosine */
518 /* itself the difference from the value 1.0 is calculated, this can be done with */
519 /* lower precision maths. */
520 /* */
521 /* 3. The value of the B2 coefficient is only calculated as a single precision value, */
522 /* small errors in this value have a combined effect on the Q and Gain but not the */
523 /* the frequency of the filter. */
524 /* */
525 /* 4. This function is entirely based on the LVEQNB_DoublePrecCoefs function */
526 /* of the n bands equalizer (LVEQNB */
527 /* */
528 /****************************************************************************************/
LVPSA_BPDoublePrecCoefs(LVM_UINT16 Fs,LVPSA_FilterParam_t * pFilterParams,BP_FLOAT_Coefs_t * pCoefficients)529 LVPSA_RETURN LVPSA_BPDoublePrecCoefs(LVM_UINT16 Fs, LVPSA_FilterParam_t* pFilterParams,
530 BP_FLOAT_Coefs_t* pCoefficients) {
531 extern LVM_FLOAT LVPSA_Float_TwoPiOnFsTable[];
532 extern LVM_FLOAT LVPSA_Float_DPCosCoef[];
533
534 /*
535 * Intermediate variables and temporary values
536 */
537 LVM_FLOAT T0;
538 LVM_FLOAT D;
539 LVM_FLOAT A0;
540 LVM_FLOAT B1;
541 LVM_FLOAT B2;
542 LVM_FLOAT Dt0;
543 LVM_FLOAT B2_Den;
544 LVM_FLOAT B2_Num;
545 LVM_FLOAT CosErr;
546 LVM_FLOAT coef;
547 LVM_FLOAT factor;
548 LVM_FLOAT t0;
549 LVM_INT16 i;
550
551 /*
552 * Get the filter definition
553 */
554 LVM_FLOAT Frequency = (LVM_FLOAT)(pFilterParams->CenterFrequency);
555 LVM_FLOAT QFactor = ((LVM_FLOAT)(pFilterParams->QFactor)) / 100;
556
557 /*
558 * Calculating the intermediate values
559 */
560 T0 = Frequency * LVPSA_Float_TwoPiOnFsTable[Fs]; /* T0 = 2 * Pi * Fc / Fs */
561 D = 3200; /* Floating point value 1.000000 (1*100*2^5) */
562 /* Force D = 1 : the function was originally used for a peaking filter.
563 The D parameter do not exist for a BandPass filter coefficients */
564
565 /*
566 * Calculate the B2 coefficient
567 */
568 Dt0 = T0 / 2048;
569 B2_Den = QFactor + Dt0;
570 B2_Num = Dt0 - QFactor;
571 B2 = B2_Num / (2 * B2_Den);
572
573 /*
574 * Calculate the cosine error by a polynomial expansion using the equation:
575 *
576 * CosErr += coef(n) * t0^n For n = 0 to 4
577 */
578 T0 = T0 * 0.994750f; /* Scale to 1.0 in 16-bit for range 0 to fs/50 */
579 t0 = T0;
580 factor = 1.0f; /* Initialise to 1.0 for the a0 coefficient */
581 CosErr = 0.0f; /* Initialise the error to zero */
582 for (i = 1; i < 5; i++) {
583 coef = LVPSA_Float_DPCosCoef[i]; /* Get the nth coefficient */
584 CosErr += factor * coef; /* The nth partial sum */
585 factor = factor * t0; /* Calculate t0^n */
586 }
587 CosErr = CosErr * 2; /* Correct the scaling */
588
589 /*
590 * Calculate the B1 and A0 coefficients
591 */
592 B1 = ((LVM_FLOAT)0.5 - B2); /* B1 = (0.5 - b2) */
593 A0 = B1 * CosErr; /* Temporary storage for (0.5 - b2) * coserr(t0) */
594 B1 -= A0; /* B1 = (0.5 - b2) * (1 - coserr(t0)) */
595 A0 = ((LVM_FLOAT)0.5 + B2) / 2; /* A0 = (0.5 + b2) / 2 */
596
597 /*
598 * Write coeff into the data structure
599 */
600 pCoefficients->A0 = A0;
601 pCoefficients->B1 = B1;
602 pCoefficients->B2 = B2;
603
604 return (LVPSA_OK);
605 }
606 /************************************************************************************/
607 /* */
608 /* FUNCTION: LVPSA_ClearFilterHistory */
609 /* */
610 /* DESCRIPTION: */
611 /* Clears the filters' data history */
612 /* */
613 /* PARAMETERS: */
614 /* pInst Pointer to the instance */
615 /* */
616 /* RETURNS: */
617 /* LVPSA_OK Always succeeds */
618 /* */
619 /* NOTES: */
620 /* */
621 /************************************************************************************/
LVPSA_ClearFilterHistory(LVPSA_InstancePr_t * pInst)622 LVPSA_RETURN LVPSA_ClearFilterHistory(LVPSA_InstancePr_t* pInst) {
623 for (size_t i = 0; i < pInst->specBiquad.size(); i++) {
624 pInst->specBiquad[i].clear();
625 }
626
627 return (LVPSA_OK);
628 }
629