Marco Zecchini
/
Example_RTOS
Rtos API example
Embed:
(wiki syntax)
Show/hide line numbers
arm_biquad_cascade_stereo_df2T_f32.c
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2010-2014 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 19. March 2015 00005 * $Revision: V.1.4.5 00006 * 00007 * Project: CMSIS DSP Library 00008 * Title: arm_biquad_cascade_stereo_df2T_f32.c 00009 * 00010 * Description: Processing function for the floating-point transposed 00011 * direct form II Biquad cascade filter. 2 channels 00012 * 00013 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0 00014 * 00015 * Redistribution and use in source and binary forms, with or without 00016 * modification, are permitted provided that the following conditions 00017 * are met: 00018 * - Redistributions of source code must retain the above copyright 00019 * notice, this list of conditions and the following disclaimer. 00020 * - Redistributions in binary form must reproduce the above copyright 00021 * notice, this list of conditions and the following disclaimer in 00022 * the documentation and/or other materials provided with the 00023 * distribution. 00024 * - Neither the name of ARM LIMITED nor the names of its contributors 00025 * may be used to endorse or promote products derived from this 00026 * software without specific prior written permission. 00027 * 00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00039 * POSSIBILITY OF SUCH DAMAGE. 00040 * -------------------------------------------------------------------- */ 00041 00042 #include "arm_math.h" 00043 00044 /** 00045 * @ingroup groupFilters 00046 */ 00047 00048 /** 00049 * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure 00050 * 00051 * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure. 00052 * The filters are implemented as a cascade of second order Biquad sections. 00053 * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions. 00054 * Only floating-point data is supported. 00055 * 00056 * This function operate on blocks of input and output data and each call to the function 00057 * processes <code>blockSize</code> samples through the filter. 00058 * <code>pSrc</code> points to the array of input data and 00059 * <code>pDst</code> points to the array of output data. 00060 * Both arrays contain <code>blockSize</code> values. 00061 * 00062 * \par Algorithm 00063 * Each Biquad stage implements a second order filter using the difference equation: 00064 * <pre> 00065 * y[n] = b0 * x[n] + d1 00066 * d1 = b1 * x[n] + a1 * y[n] + d2 00067 * d2 = b2 * x[n] + a2 * y[n] 00068 * </pre> 00069 * where d1 and d2 represent the two state values. 00070 * 00071 * \par 00072 * A Biquad filter using a transposed Direct Form II structure is shown below. 00073 * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad" 00074 * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients. 00075 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients. 00076 * Pay careful attention to the sign of the feedback coefficients. 00077 * Some design tools flip the sign of the feedback coefficients: 00078 * <pre> 00079 * y[n] = b0 * x[n] + d1; 00080 * d1 = b1 * x[n] - a1 * y[n] + d2; 00081 * d2 = b2 * x[n] - a2 * y[n]; 00082 * </pre> 00083 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library. 00084 * 00085 * \par 00086 * Higher order filters are realized as a cascade of second order sections. 00087 * <code>numStages</code> refers to the number of second order stages used. 00088 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages. 00089 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the 00090 * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>). 00091 * 00092 * \par 00093 * <code>pState</code> points to the state variable array. 00094 * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>. 00095 * The state variables are arranged in the <code>pState</code> array as: 00096 * <pre> 00097 * {d11, d12, d21, d22, ...} 00098 * </pre> 00099 * where <code>d1x</code> refers to the state variables for the first Biquad and 00100 * <code>d2x</code> refers to the state variables for the second Biquad. 00101 * The state array has a total length of <code>2*numStages</code> values. 00102 * The state variables are updated after each block of data is processed; the coefficients are untouched. 00103 * 00104 * \par 00105 * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II. 00106 * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types. 00107 * That is why the Direct Form I structure supports Q15 and Q31 data types. 00108 * The transposed Direct Form II structure, on the other hand, requires a wide dynamic range for the state variables <code>d1</code> and <code>d2</code>. 00109 * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad. 00110 * The advantage of the Direct Form II Biquad is that it requires half the number of state variables, 2 rather than 4, per Biquad stage. 00111 * 00112 * \par Instance Structure 00113 * The coefficients and state variables for a filter are stored together in an instance data structure. 00114 * A separate instance structure must be defined for each filter. 00115 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared. 00116 * 00117 * \par Init Functions 00118 * There is also an associated initialization function. 00119 * The initialization function performs following operations: 00120 * - Sets the values of the internal structure fields. 00121 * - Zeros out the values in the state buffer. 00122 * To do this manually without calling the init function, assign the follow subfields of the instance structure: 00123 * numStages, pCoeffs, pState. Also set all of the values in pState to zero. 00124 * 00125 * \par 00126 * Use of the initialization function is optional. 00127 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section. 00128 * To place an instance structure into a const data section, the instance structure must be manually initialized. 00129 * Set the values in the state buffer to zeros before static initialization. 00130 * For example, to statically initialize the instance structure use 00131 * <pre> 00132 * arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs}; 00133 * </pre> 00134 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer. 00135 * <code>pCoeffs</code> is the address of the coefficient buffer; 00136 * 00137 */ 00138 00139 /** 00140 * @addtogroup BiquadCascadeDF2T 00141 * @{ 00142 */ 00143 00144 /** 00145 * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter. 00146 * @param[in] *S points to an instance of the filter data structure. 00147 * @param[in] *pSrc points to the block of input data. 00148 * @param[out] *pDst points to the block of output data 00149 * @param[in] blockSize number of samples to process. 00150 * @return none. 00151 */ 00152 00153 00154 LOW_OPTIMIZATION_ENTER 00155 void arm_biquad_cascade_stereo_df2T_f32( 00156 const arm_biquad_cascade_stereo_df2T_instance_f32 * S, 00157 float32_t * pSrc, 00158 float32_t * pDst, 00159 uint32_t blockSize) 00160 { 00161 00162 float32_t *pIn = pSrc; /* source pointer */ 00163 float32_t *pOut = pDst; /* destination pointer */ 00164 float32_t *pState = S->pState; /* State pointer */ 00165 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ 00166 float32_t acc1a, acc1b; /* accumulator */ 00167 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00168 float32_t Xn1a, Xn1b; /* temporary input */ 00169 float32_t d1a, d2a, d1b, d2b; /* state variables */ 00170 uint32_t sample, stage = S->numStages; /* loop counters */ 00171 00172 #if defined(ARM_MATH_CM7) 00173 00174 float32_t Xn2a, Xn3a, Xn4a, Xn5a, Xn6a, Xn7a, Xn8a; /* Input State variables */ 00175 float32_t Xn2b, Xn3b, Xn4b, Xn5b, Xn6b, Xn7b, Xn8b; /* Input State variables */ 00176 float32_t acc2a, acc3a, acc4a, acc5a, acc6a, acc7a, acc8a; /* Simulates the accumulator */ 00177 float32_t acc2b, acc3b, acc4b, acc5b, acc6b, acc7b, acc8b; /* Simulates the accumulator */ 00178 00179 do 00180 { 00181 /* Reading the coefficients */ 00182 b0 = pCoeffs[0]; 00183 b1 = pCoeffs[1]; 00184 b2 = pCoeffs[2]; 00185 a1 = pCoeffs[3]; 00186 /* Apply loop unrolling and compute 8 output values simultaneously. */ 00187 sample = blockSize >> 3u; 00188 a2 = pCoeffs[4]; 00189 00190 /*Reading the state values */ 00191 d1a = pState[0]; 00192 d2a = pState[1]; 00193 d1b = pState[2]; 00194 d2b = pState[3]; 00195 00196 pCoeffs += 5u; 00197 00198 /* First part of the processing with loop unrolling. Compute 8 outputs at a time. 00199 ** a second loop below computes the remaining 1 to 7 samples. */ 00200 while(sample > 0u) { 00201 00202 /* y[n] = b0 * x[n] + d1 */ 00203 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00204 /* d2 = b2 * x[n] + a2 * y[n] */ 00205 00206 /* Read the first 2 inputs. 2 cycles */ 00207 Xn1a = pIn[0 ]; 00208 Xn1b = pIn[1 ]; 00209 00210 /* Sample 1. 5 cycles */ 00211 Xn2a = pIn[2 ]; 00212 acc1a = b0 * Xn1a + d1a; 00213 00214 Xn2b = pIn[3 ]; 00215 d1a = b1 * Xn1a + d2a; 00216 00217 Xn3a = pIn[4 ]; 00218 d2a = b2 * Xn1a; 00219 00220 Xn3b = pIn[5 ]; 00221 d1a += a1 * acc1a; 00222 00223 Xn4a = pIn[6 ]; 00224 d2a += a2 * acc1a; 00225 00226 /* Sample 2. 5 cycles */ 00227 Xn4b = pIn[7 ]; 00228 acc1b = b0 * Xn1b + d1b; 00229 00230 Xn5a = pIn[8 ]; 00231 d1b = b1 * Xn1b + d2b; 00232 00233 Xn5b = pIn[9 ]; 00234 d2b = b2 * Xn1b; 00235 00236 Xn6a = pIn[10]; 00237 d1b += a1 * acc1b; 00238 00239 Xn6b = pIn[11]; 00240 d2b += a2 * acc1b; 00241 00242 /* Sample 3. 5 cycles */ 00243 Xn7a = pIn[12]; 00244 acc2a = b0 * Xn2a + d1a; 00245 00246 Xn7b = pIn[13]; 00247 d1a = b1 * Xn2a + d2a; 00248 00249 Xn8a = pIn[14]; 00250 d2a = b2 * Xn2a; 00251 00252 Xn8b = pIn[15]; 00253 d1a += a1 * acc2a; 00254 00255 pIn += 16; 00256 d2a += a2 * acc2a; 00257 00258 /* Sample 4. 5 cycles */ 00259 acc2b = b0 * Xn2b + d1b; 00260 d1b = b1 * Xn2b + d2b; 00261 d2b = b2 * Xn2b; 00262 d1b += a1 * acc2b; 00263 d2b += a2 * acc2b; 00264 00265 /* Sample 5. 5 cycles */ 00266 acc3a = b0 * Xn3a + d1a; 00267 d1a = b1 * Xn3a + d2a; 00268 d2a = b2 * Xn3a; 00269 d1a += a1 * acc3a; 00270 d2a += a2 * acc3a; 00271 00272 /* Sample 6. 5 cycles */ 00273 acc3b = b0 * Xn3b + d1b; 00274 d1b = b1 * Xn3b + d2b; 00275 d2b = b2 * Xn3b; 00276 d1b += a1 * acc3b; 00277 d2b += a2 * acc3b; 00278 00279 /* Sample 7. 5 cycles */ 00280 acc4a = b0 * Xn4a + d1a; 00281 d1a = b1 * Xn4a + d2a; 00282 d2a = b2 * Xn4a; 00283 d1a += a1 * acc4a; 00284 d2a += a2 * acc4a; 00285 00286 /* Sample 8. 5 cycles */ 00287 acc4b = b0 * Xn4b + d1b; 00288 d1b = b1 * Xn4b + d2b; 00289 d2b = b2 * Xn4b; 00290 d1b += a1 * acc4b; 00291 d2b += a2 * acc4b; 00292 00293 /* Sample 9. 5 cycles */ 00294 acc5a = b0 * Xn5a + d1a; 00295 d1a = b1 * Xn5a + d2a; 00296 d2a = b2 * Xn5a; 00297 d1a += a1 * acc5a; 00298 d2a += a2 * acc5a; 00299 00300 /* Sample 10. 5 cycles */ 00301 acc5b = b0 * Xn5b + d1b; 00302 d1b = b1 * Xn5b + d2b; 00303 d2b = b2 * Xn5b; 00304 d1b += a1 * acc5b; 00305 d2b += a2 * acc5b; 00306 00307 /* Sample 11. 5 cycles */ 00308 acc6a = b0 * Xn6a + d1a; 00309 d1a = b1 * Xn6a + d2a; 00310 d2a = b2 * Xn6a; 00311 d1a += a1 * acc6a; 00312 d2a += a2 * acc6a; 00313 00314 /* Sample 12. 5 cycles */ 00315 acc6b = b0 * Xn6b + d1b; 00316 d1b = b1 * Xn6b + d2b; 00317 d2b = b2 * Xn6b; 00318 d1b += a1 * acc6b; 00319 d2b += a2 * acc6b; 00320 00321 /* Sample 13. 5 cycles */ 00322 acc7a = b0 * Xn7a + d1a; 00323 d1a = b1 * Xn7a + d2a; 00324 00325 pOut[0 ] = acc1a ; 00326 d2a = b2 * Xn7a; 00327 00328 pOut[1 ] = acc1b ; 00329 d1a += a1 * acc7a; 00330 00331 pOut[2 ] = acc2a ; 00332 d2a += a2 * acc7a; 00333 00334 /* Sample 14. 5 cycles */ 00335 pOut[3 ] = acc2b ; 00336 acc7b = b0 * Xn7b + d1b; 00337 00338 pOut[4 ] = acc3a ; 00339 d1b = b1 * Xn7b + d2b; 00340 00341 pOut[5 ] = acc3b ; 00342 d2b = b2 * Xn7b; 00343 00344 pOut[6 ] = acc4a ; 00345 d1b += a1 * acc7b; 00346 00347 pOut[7 ] = acc4b ; 00348 d2b += a2 * acc7b; 00349 00350 /* Sample 15. 5 cycles */ 00351 pOut[8 ] = acc5a ; 00352 acc8a = b0 * Xn8a + d1a; 00353 00354 pOut[9 ] = acc5b; 00355 d1a = b1 * Xn8a + d2a; 00356 00357 pOut[10] = acc6a; 00358 d2a = b2 * Xn8a; 00359 00360 pOut[11] = acc6b; 00361 d1a += a1 * acc8a; 00362 00363 pOut[12] = acc7a; 00364 d2a += a2 * acc8a; 00365 00366 /* Sample 16. 5 cycles */ 00367 pOut[13] = acc7b; 00368 acc8b = b0 * Xn8b + d1b; 00369 00370 pOut[14] = acc8a; 00371 d1b = b1 * Xn8b + d2b; 00372 00373 pOut[15] = acc8b; 00374 d2b = b2 * Xn8b; 00375 00376 sample--; 00377 d1b += a1 * acc8b; 00378 00379 pOut += 16; 00380 d2b += a2 * acc8b; 00381 } 00382 00383 sample = blockSize & 0x7u; 00384 while(sample > 0u) { 00385 /* Read the input */ 00386 Xn1a = *pIn++; //Channel a 00387 Xn1b = *pIn++; //Channel b 00388 00389 /* y[n] = b0 * x[n] + d1 */ 00390 acc1a = (b0 * Xn1a) + d1a; 00391 acc1b = (b0 * Xn1b) + d1b; 00392 00393 /* Store the result in the accumulator in the destination buffer. */ 00394 *pOut++ = acc1a; 00395 *pOut++ = acc1b; 00396 00397 /* Every time after the output is computed state should be updated. */ 00398 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00399 d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a; 00400 d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b; 00401 00402 /* d2 = b2 * x[n] + a2 * y[n] */ 00403 d2a = (b2 * Xn1a) + (a2 * acc1a); 00404 d2b = (b2 * Xn1b) + (a2 * acc1b); 00405 00406 sample--; 00407 } 00408 00409 /* Store the updated state variables back into the state array */ 00410 pState[0] = d1a; 00411 pState[1] = d2a; 00412 00413 pState[2] = d1b; 00414 pState[3] = d2b; 00415 00416 /* The current stage input is given as the output to the next stage */ 00417 pIn = pDst; 00418 /* decrement the loop counter */ 00419 stage--; 00420 00421 pState += 4u; 00422 /*Reset the output working pointer */ 00423 pOut = pDst; 00424 00425 } while(stage > 0u); 00426 00427 #elif defined(ARM_MATH_CM0_FAMILY) 00428 00429 /* Run the below code for Cortex-M0 */ 00430 00431 do 00432 { 00433 /* Reading the coefficients */ 00434 b0 = *pCoeffs++; 00435 b1 = *pCoeffs++; 00436 b2 = *pCoeffs++; 00437 a1 = *pCoeffs++; 00438 a2 = *pCoeffs++; 00439 00440 /*Reading the state values */ 00441 d1a = pState[0]; 00442 d2a = pState[1]; 00443 d1b = pState[2]; 00444 d2b = pState[3]; 00445 00446 00447 sample = blockSize; 00448 00449 while(sample > 0u) 00450 { 00451 /* Read the input */ 00452 Xn1a = *pIn++; //Channel a 00453 Xn1b = *pIn++; //Channel b 00454 00455 /* y[n] = b0 * x[n] + d1 */ 00456 acc1a = (b0 * Xn1a) + d1a; 00457 acc1b = (b0 * Xn1b) + d1b; 00458 00459 /* Store the result in the accumulator in the destination buffer. */ 00460 *pOut++ = acc1a; 00461 *pOut++ = acc1b; 00462 00463 /* Every time after the output is computed state should be updated. */ 00464 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00465 d1a = ((b1 * Xn1a) + (a1 * acc1a)) + d2a; 00466 d1b = ((b1 * Xn1b) + (a1 * acc1b)) + d2b; 00467 00468 /* d2 = b2 * x[n] + a2 * y[n] */ 00469 d2a = (b2 * Xn1a) + (a2 * acc1a); 00470 d2b = (b2 * Xn1b) + (a2 * acc1b); 00471 00472 /* decrement the loop counter */ 00473 sample--; 00474 } 00475 00476 /* Store the updated state variables back into the state array */ 00477 *pState++ = d1a; 00478 *pState++ = d2a; 00479 *pState++ = d1b; 00480 *pState++ = d2b; 00481 00482 /* The current stage input is given as the output to the next stage */ 00483 pIn = pDst; 00484 00485 /*Reset the output working pointer */ 00486 pOut = pDst; 00487 00488 /* decrement the loop counter */ 00489 stage--; 00490 00491 } while(stage > 0u); 00492 00493 #else 00494 00495 float32_t Xn2a, Xn3a, Xn4a; /* Input State variables */ 00496 float32_t Xn2b, Xn3b, Xn4b; /* Input State variables */ 00497 float32_t acc2a, acc3a, acc4a; /* accumulator */ 00498 float32_t acc2b, acc3b, acc4b; /* accumulator */ 00499 float32_t p0a, p1a, p2a, p3a, p4a, A1a; 00500 float32_t p0b, p1b, p2b, p3b, p4b, A1b; 00501 00502 /* Run the below code for Cortex-M4 and Cortex-M3 */ 00503 do 00504 { 00505 /* Reading the coefficients */ 00506 b0 = *pCoeffs++; 00507 b1 = *pCoeffs++; 00508 b2 = *pCoeffs++; 00509 a1 = *pCoeffs++; 00510 a2 = *pCoeffs++; 00511 00512 /*Reading the state values */ 00513 d1a = pState[0]; 00514 d2a = pState[1]; 00515 d1b = pState[2]; 00516 d2b = pState[3]; 00517 00518 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00519 sample = blockSize >> 2u; 00520 00521 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00522 ** a second loop below computes the remaining 1 to 3 samples. */ 00523 while(sample > 0u) { 00524 00525 /* y[n] = b0 * x[n] + d1 */ 00526 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00527 /* d2 = b2 * x[n] + a2 * y[n] */ 00528 00529 /* Read the four inputs */ 00530 Xn1a = pIn[0]; 00531 Xn1b = pIn[1]; 00532 Xn2a = pIn[2]; 00533 Xn2b = pIn[3]; 00534 Xn3a = pIn[4]; 00535 Xn3b = pIn[5]; 00536 Xn4a = pIn[6]; 00537 Xn4b = pIn[7]; 00538 pIn += 8; 00539 00540 p0a = b0 * Xn1a; 00541 p0b = b0 * Xn1b; 00542 p1a = b1 * Xn1a; 00543 p1b = b1 * Xn1b; 00544 acc1a = p0a + d1a; 00545 acc1b = p0b + d1b; 00546 p0a = b0 * Xn2a; 00547 p0b = b0 * Xn2b; 00548 p3a = a1 * acc1a; 00549 p3b = a1 * acc1b; 00550 p2a = b2 * Xn1a; 00551 p2b = b2 * Xn1b; 00552 A1a = p1a + p3a; 00553 A1b = p1b + p3b; 00554 p4a = a2 * acc1a; 00555 p4b = a2 * acc1b; 00556 d1a = A1a + d2a; 00557 d1b = A1b + d2b; 00558 d2a = p2a + p4a; 00559 d2b = p2b + p4b; 00560 00561 p1a = b1 * Xn2a; 00562 p1b = b1 * Xn2b; 00563 acc2a = p0a + d1a; 00564 acc2b = p0b + d1b; 00565 p0a = b0 * Xn3a; 00566 p0b = b0 * Xn3b; 00567 p3a = a1 * acc2a; 00568 p3b = a1 * acc2b; 00569 p2a = b2 * Xn2a; 00570 p2b = b2 * Xn2b; 00571 A1a = p1a + p3a; 00572 A1b = p1b + p3b; 00573 p4a = a2 * acc2a; 00574 p4b = a2 * acc2b; 00575 d1a = A1a + d2a; 00576 d1b = A1b + d2b; 00577 d2a = p2a + p4a; 00578 d2b = p2b + p4b; 00579 00580 p1a = b1 * Xn3a; 00581 p1b = b1 * Xn3b; 00582 acc3a = p0a + d1a; 00583 acc3b = p0b + d1b; 00584 p0a = b0 * Xn4a; 00585 p0b = b0 * Xn4b; 00586 p3a = a1 * acc3a; 00587 p3b = a1 * acc3b; 00588 p2a = b2 * Xn3a; 00589 p2b = b2 * Xn3b; 00590 A1a = p1a + p3a; 00591 A1b = p1b + p3b; 00592 p4a = a2 * acc3a; 00593 p4b = a2 * acc3b; 00594 d1a = A1a + d2a; 00595 d1b = A1b + d2b; 00596 d2a = p2a + p4a; 00597 d2b = p2b + p4b; 00598 00599 acc4a = p0a + d1a; 00600 acc4b = p0b + d1b; 00601 p1a = b1 * Xn4a; 00602 p1b = b1 * Xn4b; 00603 p3a = a1 * acc4a; 00604 p3b = a1 * acc4b; 00605 p2a = b2 * Xn4a; 00606 p2b = b2 * Xn4b; 00607 A1a = p1a + p3a; 00608 A1b = p1b + p3b; 00609 p4a = a2 * acc4a; 00610 p4b = a2 * acc4b; 00611 d1a = A1a + d2a; 00612 d1b = A1b + d2b; 00613 d2a = p2a + p4a; 00614 d2b = p2b + p4b; 00615 00616 pOut[0] = acc1a; 00617 pOut[1] = acc1b; 00618 pOut[2] = acc2a; 00619 pOut[3] = acc2b; 00620 pOut[4] = acc3a; 00621 pOut[5] = acc3b; 00622 pOut[6] = acc4a; 00623 pOut[7] = acc4b; 00624 pOut += 8; 00625 00626 sample--; 00627 } 00628 00629 sample = blockSize & 0x3u; 00630 while(sample > 0u) { 00631 Xn1a = *pIn++; 00632 Xn1b = *pIn++; 00633 00634 p0a = b0 * Xn1a; 00635 p0b = b0 * Xn1b; 00636 p1a = b1 * Xn1a; 00637 p1b = b1 * Xn1b; 00638 acc1a = p0a + d1a; 00639 acc1b = p0b + d1b; 00640 p3a = a1 * acc1a; 00641 p3b = a1 * acc1b; 00642 p2a = b2 * Xn1a; 00643 p2b = b2 * Xn1b; 00644 A1a = p1a + p3a; 00645 A1b = p1b + p3b; 00646 p4a = a2 * acc1a; 00647 p4b = a2 * acc1b; 00648 d1a = A1a + d2a; 00649 d1b = A1b + d2b; 00650 d2a = p2a + p4a; 00651 d2b = p2b + p4b; 00652 00653 *pOut++ = acc1a; 00654 *pOut++ = acc1b; 00655 00656 sample--; 00657 } 00658 00659 /* Store the updated state variables back into the state array */ 00660 *pState++ = d1a; 00661 *pState++ = d2a; 00662 *pState++ = d1b; 00663 *pState++ = d2b; 00664 00665 /* The current stage input is given as the output to the next stage */ 00666 pIn = pDst; 00667 00668 /*Reset the output working pointer */ 00669 pOut = pDst; 00670 00671 /* decrement the loop counter */ 00672 stage--; 00673 00674 } while(stage > 0u); 00675 00676 #endif 00677 00678 } 00679 LOW_OPTIMIZATION_EXIT 00680 00681 /** 00682 * @} end of BiquadCascadeDF2T group 00683 */
Generated on Sun Jul 17 2022 08:25:18 by 1.7.2