arm_biquad_cascade_df1_f32.c
Go to the documentation of this file.
00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2011 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 15. December 2011 00005 * $Revision: V2.0.0 00006 * 00007 * Project: Cortex-R DSP Library 00008 * Title: arm_biquad_cascade_df1_f32.c 00009 * 00010 * Description: Processing function for the 00011 * floating-point Biquad cascade DirectFormI(DF1) filter. 00012 * 00013 * Target Processor: Cortex-R4/R5 00014 * 00015 * Version 1.0.0 2011/03/08 00016 * Alpha release. 00017 * 00018 * Version 1.0.1 2011/09/30 00019 * Beta release. 00020 * 00021 * Version 2.0.0 2011/12/15 00022 * Final release. 00023 * 00024 * -------------------------------------------------------------------- */ 00025 00026 #include "arm_math.h" 00027 00159 void arm_biquad_cascade_df1_f32( 00160 const arm_biquad_casd_df1_inst_f32 * S, 00161 float32_t * pSrc, 00162 float32_t * pDst, 00163 uint32_t blockSize) 00164 { 00165 float32_t *pIn = pSrc; /* source pointer */ 00166 float32_t *pOut = pDst; /* destination pointer */ 00167 float32_t *pState = S->pState; /* pState pointer */ 00168 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ 00169 float32_t acc; /* Simulates the accumulator */ 00170 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00171 float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */ 00172 float32_t Xn; /* temporary input */ 00173 uint32_t sample, stage = S->numStages; /* loop counters */ 00174 float32_t X0, X1, X2; /* Temporary variables to read inputs */ 00175 float32_t Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7; /* Temporary variables to write outputs */ 00176 00177 do 00178 { 00179 /* Reading the coefficients */ 00180 b0 = *pCoeffs++; 00181 b1 = *pCoeffs++; 00182 b2 = *pCoeffs++; 00183 a1 = *pCoeffs++; 00184 a2 = *pCoeffs++; 00185 00186 /* Reading the pState values */ 00187 Xn2 = pState[0]; 00188 Xn1 = pState[1]; 00189 Yn2 = pState[2]; 00190 Yn1 = pState[3]; 00191 00192 /*; Equation to calculate output of the filter is as follows: 00193 ; y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00194 ; y[0] = b0*x[0] + b1*x[-1] + b2*x[-2] + a1*y[-1] + a2*y[-2] 00195 ; y[1] = b0*x[1] + b1*x[0] + b2*x[-1] + a1*y[0] + a2*y[-1] 00196 ; y[2] = b0*x[2] + b1*x[1] + b2*x[0] + a1*y[1] + a2*y[0] 00197 ; y[3] = b0*x[3] + b1*x[2] + b2*x[1] + a1*y[2] + a2*y[1] 00198 ; 00199 ; y[4] = b0*x[4] + b1*x[3] + b2*x[2] + a1*y[3] + a2*y[2] 00200 ; y[5] = b0*x[5] + b1*x[4] + b2*x[3] + a1*y[4] + a2*y[3] 00201 ; y[6] = b0*x[6] + b1*x[5] + b2*x[4] + a1*y[5] + a2*y[4] 00202 ; y[7] = b0*x[7] + b1*x[6] + b2*x[5] + a1*y[6] + a2*y[5]*/ 00203 00204 /* First part of the processing with loop unrolling. Compute 8 outputs at a time. 00205 ** a second loop below computes the remaining 1 to 7 samples. */ 00206 sample = blockSize >> 3u; 00207 00208 if(sample > 0) 00209 { 00210 /* 8 samples calculation is done before and after loop processing */ 00211 sample = sample - 1u; 00212 00213 /* 00214 ;y[0] = b0*x[0] + b1*x[-1] + b2*x[-2] + a2*y[-2] + a1 * y[-1] //ch1:0 00215 ;y[1] = b0*x[1] + b1*x[0] + b2*x[-1] + a2 * y[-1] 00216 ;y[2] = b0*x[2] + b1*x[1] + b2*x[0] 00217 ;y[3] = b0*x[3] + b1*x[2] + b2*x[1] */ 00218 00219 /* Read x[0] */ 00220 X2 = *pIn++; 00221 00222 00223 /* y[0] = b2*x[-2] */ 00224 Y0 = (Xn2 * b2); 00225 /* y[1] = b2*x[-1] */ 00226 Y1 = (Xn1 * b2); 00227 /* y[2] = b2*x[0] */ 00228 Y2 = (X2 * b2); 00229 00230 /* Read x[1] */ 00231 X0 = *pIn++; 00232 00233 00234 /* y[3] = b2*x[1] */ 00235 Y3 = (X0 * b2); 00236 00237 /* y[0] += b1*x[-1] */ 00238 Y0 += (Xn1 * b1); 00239 /* y[1] += b1*x[0] */ 00240 Y1 += (X2 * b1); 00241 /* y[2] += b1*x[1] */ 00242 Y2 += (X0 * b1); 00243 00244 /* Read x[2] */ 00245 X1 = *pIn++; 00246 00247 00248 /* y[0] += b0*x[0] */ 00249 Y0 += (X2 * b0); 00250 /* y[3] += b1*x[2] */ 00251 Y3 += (X1 * b1); 00252 /* y[1] += b0*x[0] */ 00253 Y1 += (X0 * b0); 00254 /* y[2] += b0*x[2] */ 00255 Y2 += (X1 * b0); 00256 00257 00258 /* y[0] += b1*x[-1] */ 00259 Y0 += (Yn2 * a2); 00260 00261 /* Read x[3] */ 00262 X2 = *pIn++; 00263 00264 00265 /* y[3] += b0*x[3] */ 00266 Y3 += (X2 * b0); 00267 /* y[1] += a2*y[-1] */ 00268 Y1 += (Yn1 * a2); 00269 /* y[0] += a1*y[-1] */ 00270 Y0 += (Yn1 * a1); 00271 00272 while(sample > 0u) 00273 { 00274 00275 /* 00276 ;y[1] += a1*y[0] 00277 ;y[2] += a1*y[1] + a2*y[0] 00278 ;y[3] += a1*y[2] + a2*y[1] 00279 ;y[4] = b0*x[4] + b1*x[3] + b2*x[2] + a2*y[2] 00280 ;y[5] = b0*x[5] + b1*x[4] + b2*x[3] //x in eq of y is for ch1:0 00281 ;y[6] = b0*x[6] + b1*x[5] + b2*x[4] 00282 ;y[7] = b0*x[7] + b1*x[6] + b2*x[5] 00283 */ 00284 00285 /* Read x[4] */ 00286 X0 = *pIn++; 00287 00288 00289 /* y[4] = b2*x[2] */ 00290 Y4 = (X1 * b2); 00291 /* y[5] = b2*x[3] */ 00292 Y5 = (X2 * b2); 00293 /* y[6] = b2*x[4] */ 00294 Y6 = (X0 * b2); 00295 00296 /* Read x[5] */ 00297 X1 = *pIn++; 00298 00299 00300 Y1 += (Y0 * a1); 00301 Y2 += (Y0 * a2); 00302 00303 /* y[4] += b1*x[3] */ 00304 Y4 += (X2 * b1); 00305 /* y[5] += b1*x[4] */ 00306 Y5 += (X0 * b1); 00307 /* y[6] += b1*x[5] */ 00308 Y6 += (X1 * b1); 00309 00310 Y7 = (X1 * b2); 00311 00312 /* Read x[6] */ 00313 X2 = *pIn++; 00314 00315 00316 /* Write y[0] */ 00317 *pOut++ = Y0; 00318 00319 /* y[3] += a2*y[0] */ 00320 Y3 += (Y1 * a2); 00321 /* y[2] += a1*y[1] */ 00322 Y2 += (Y1 * a1); 00323 00324 /* y[4] += b0*x[4] */ 00325 Y4 += (X0 * b0); 00326 /* y[5] += b0*x[5] */ 00327 Y5 += (X1 * b0); 00328 /* y[6] += b0*x[6] */ 00329 Y6 += (X2 * b0); 00330 00331 /* y[3] += a1*y[2] */ 00332 Y3 += (Y2 * a1); 00333 /* y[4] += a2*y[2] */ 00334 Y4 += (Y2 * a2); 00335 /* y[7] += b1*x[6] */ 00336 Y7 += (X2 * b1); 00337 00338 X1 = X2; 00339 00340 /* Read x[7] */ 00341 X2 = *pIn++; 00342 00343 00344 /* Write y[1] */ 00345 *pOut++ = Y1; 00346 00347 /* y[4] += a1*y[3] */ 00348 Y4 += (Y3 * a1); 00349 /* y[5] += a2*y[3] */ 00350 Y5 += (Y3 * a2); 00351 /* y[7] += b0*x[7] */ 00352 Y7 += (X2 * b0); 00353 00354 /* Write y[2] */ 00355 *pOut++ = Y2; 00356 00357 00358 /* Write y[3] */ 00359 *pOut++ = Y3; 00360 00361 00362 X0 = *pIn++; 00363 00364 00365 /* 00366 ;y[0] = b0*x[0] + b1*x[-1] + b2*x[-2] + a2*y[-2] + a1 * y[-1] //ch1:0 00367 ;y[1] = b0*x[1] + b1*x[0] + b2*x[-1] + a2 * y[-1] 00368 ;y[2] = b0*x[2] + b1*x[1] + b2*x[0] 00369 ;y[3] = b0*x[3] + b1*x[2] + b2*x[1] */ 00370 00371 Y0 = (X1 * b2); 00372 Y1 = (X2 * b2); 00373 Y2 = (X0 * b2); 00374 00375 X1 = *pIn++; 00376 00377 00378 Y5 += (Y4 * a1); 00379 Y6 += (Y4 * a2); 00380 00381 Y0 += (X2 * b1); 00382 Y1 += (X0 * b1); 00383 Y2 += (X1 * b1); 00384 00385 Y3 = (X1 * b2); 00386 00387 X2 = *pIn++; 00388 00389 00390 /* Write y[4] */ 00391 *pOut++ = Y4; 00392 00393 00394 Y7 += (Y5 * a2); 00395 Y6 += (Y5 * a1); 00396 00397 Y0 += (X0 * b0); 00398 Y1 += (X1 * b0); 00399 Y2 += (X2 * b0); 00400 00401 Y7 += (Y6 * a1); 00402 Y0 += (Y6 * a2); 00403 Y3 += (X2 * b1); 00404 00405 X1 = X2; 00406 00407 X2 = *pIn++; 00408 00409 00410 /* Write y[5] */ 00411 *pOut++ = Y5; 00412 00413 00414 Y0 += (Y7 * a1); 00415 Y1 += (Y7 * a2); 00416 Y3 += (X2 * b0); 00417 00418 /* Write y[6] */ 00419 *pOut++ = Y6; 00420 00421 00422 /* Write y[7] */ 00423 *pOut++ = Y7; 00424 00425 00426 00427 /* decrement the loop counter */ 00428 sample--; 00429 00430 00431 } 00432 00433 /* 00434 ;y[1] += a1*y[0] 00435 ;y[2] += a1*y[1] + a2*y[0] 00436 ;y[3] += a1*y[2] + a2*y[1] 00437 ;y[4] = b0*x[4] + b1*x[3] + b2*x[2] + a2*y[2] 00438 ;y[5] = b0*x[5] + b1*x[4] + b2*x[3] //x in eq of y is for ch1:0 00439 ;y[6] = b0*x[6] + b1*x[5] + b2*x[4] 00440 ;y[7] = b0*x[7] + b1*x[6] + b2*x[5] 00441 */ 00442 00443 X0 = *pIn++; 00444 00445 00446 Y4 = (X1 * b2); 00447 Y5 = (X2 * b2); 00448 Y6 = (X0 * b2); 00449 00450 00451 X1 = *pIn++; 00452 00453 00454 Y1 += (Y0 * a1); 00455 Y2 += (Y0 * a2); 00456 00457 Y4 += (X2 * b1); 00458 Y5 += (X0 * b1); 00459 Y6 += (X1 * b1); 00460 00461 Y7 = (X1 * b2); 00462 00463 X2 = *pIn++; 00464 00465 00466 *pOut++ = Y0; 00467 00468 00469 Y3 += (Y1 * a2); 00470 Y2 += (Y1 * a1); 00471 00472 Y4 += (X0 * b0); 00473 Y5 += (X1 * b0); 00474 Y6 += (X2 * b0); 00475 00476 Y3 += (Y2 * a1); 00477 Y4 += (Y2 * a2); 00478 00479 Y7 += (X2 * b1); 00480 00481 X1 = X2; 00482 00483 X2 = *pIn++; 00484 00485 00486 *pOut++ = Y1; 00487 00488 00489 Y4 += (Y3 * a1); 00490 Y5 += (Y3 * a2); 00491 Y7 += (X2 * b0); 00492 00493 *pOut++ = Y2; 00494 00495 00496 *pOut++ = Y3; 00497 00498 00499 Y5 += (Y4 * a1); 00500 Y6 += (Y4 * a2); 00501 Y6 += (Y5 * a1); 00502 Y7 += (Y5 * a2); 00503 Y7 += (Y6 * a1); 00504 00505 *pOut++ = Y4; 00506 00507 00508 *pOut++ = Y5; 00509 00510 00511 *pOut++ = Y6; 00512 00513 00514 *pOut++ = Y7; 00515 00516 00517 00518 Xn2 = X1; 00519 Xn1 = X2; 00520 Yn2 = Y6; 00521 Yn1 = Y7; 00522 00523 } 00524 00525 /* If the blockSize is not a multiple of 8, compute any remaining output samples here. 00526 ** No loop unrolling is used. */ 00527 sample = (blockSize & 0x7u); 00528 00529 while(sample > 0u) 00530 { 00531 /* Read the input */ 00532 Xn = *pIn++; 00533 00534 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00535 acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00536 00537 /* Store the result in the accumulator in the destination buffer. */ 00538 *pOut++ = acc; 00539 00540 /* Every time after the output is computed state should be updated. */ 00541 /* The states should be updated as: */ 00542 /* Xn2 = Xn1 */ 00543 /* Xn1 = Xn */ 00544 /* Yn2 = Yn1 */ 00545 /* Yn1 = acc */ 00546 Xn2 = Xn1; 00547 Xn1 = Xn; 00548 Yn2 = Yn1; 00549 Yn1 = acc; 00550 00551 /* decrement the loop counter */ 00552 sample--; 00553 00554 } 00555 00556 /* Store the updated state variables back into the pState array */ 00557 *pState++ = Xn2; 00558 *pState++ = Xn1; 00559 *pState++ = Yn2; 00560 *pState++ = Yn1; 00561 00562 /* The first stage goes from the input buffer to the output buffer. */ 00563 /* Subsequent numStages occur in-place in the output buffer */ 00564 pIn = pDst; 00565 00566 /* Reset the output pointer */ 00567 pOut = pDst; 00568 00569 /* decrement the loop counter */ 00570 stage--; 00571 00572 } while(stage > 0u); 00573 00574 00575 00576 } 00577 00578
