Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of mbed-os by
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 Tue Jul 12 2022 13:15:19 by
