arm_biquad_cascade_df2T_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_df2T_f32.c 00009 * 00010 * Description: Processing function for the floating-point transposed 00011 * direct form II Biquad cascade 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 #include "arm_math.h" 00026 00134 void arm_biquad_cascade_df2T_f32( 00135 const arm_biquad_cascade_df2T_instance_f32 * S, 00136 float32_t * pSrc, 00137 float32_t * pDst, 00138 uint32_t blockSize) 00139 { 00140 00141 float32_t *pIn = pSrc; /* source pointer */ 00142 float32_t *pOut = pDst; /* destination pointer */ 00143 float32_t *pState = S->pState; /* State pointer */ 00144 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ 00145 float32_t acc0; /* accumulator */ 00146 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00147 float32_t Xn; /* temporary input */ 00148 float32_t d1, d2; /* state variables */ 00149 uint32_t sample, stage = S->numStages; /* loop counters */ 00150 float32_t Xn1, Xn2; /* Input State variables */ 00151 float32_t acc1; /* accumulator */ 00152 00153 do 00154 { 00155 /* Reading the coefficients */ 00156 b0 = *pCoeffs++; 00157 b1 = *pCoeffs++; 00158 b2 = *pCoeffs++; 00159 a1 = *pCoeffs++; 00160 a2 = *pCoeffs++; 00161 00162 /*Reading the state values */ 00163 d1 = pState[0]; 00164 d2 = pState[1]; 00165 00166 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00167 sample = blockSize >> 2u; 00168 00169 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00170 ** a second loop below computes the remaining 1 to 3 samples. */ 00171 while(sample > 0u) 00172 { 00173 00174 /* y[n] = b0 * x[n] + d1 */ 00175 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00176 /* d2 = b2 * x[n] + a2 * y[n] */ 00177 00178 /* Read the first input */ 00179 Xn1 = *pIn++; 00180 00181 /* y[n] = b0 * x[n] + d1 */ 00182 acc0 = (b0 * Xn1) + d1; 00183 00184 /* d1 = b1 * x[n] + d2 */ 00185 d1 = (b1 * Xn1) + d2; 00186 00187 /* d2 = b2 * x[n] */ 00188 d2 = (b2 * Xn1); 00189 00190 /* Read the second input */ 00191 Xn2 = *pIn++; 00192 00193 /* d1 = b1 * x[n] + a1 * y[n] */ 00194 d1 = (a1 * acc0) + d1; 00195 00196 /* Store the result in the accumulator in the destination buffer. */ 00197 *pOut++ = acc0; 00198 00199 d2 = (a2 * acc0) + d2; 00200 00201 /* y[n] = b0 * x[n] + d1 */ 00202 acc1 = (b0 * Xn2) + d1; 00203 00204 /* Read the third input */ 00205 Xn1 = *pIn++; 00206 00207 d1 = (b1 * Xn2) + d2; 00208 00209 d2 = (b2 * Xn2); 00210 00211 /* Store the result in the accumulator in the destination buffer. */ 00212 *pOut++ = acc1; 00213 00214 d1 = (a1 * acc1) + d1; 00215 00216 d2 = (a2 * acc1) + d2; 00217 00218 /* y[n] = b0 * x[n] + d1 */ 00219 acc0 = (b0 * Xn1) + d1; 00220 00221 d1 = (b1 * Xn1) + d2; 00222 00223 d2 = (b2 * Xn1); 00224 00225 /* Read the fourth input */ 00226 Xn2 = *pIn++; 00227 00228 d1 = (a1 * acc0) + d1; 00229 00230 /* Store the result in the accumulator in the destination buffer. */ 00231 *pOut++ = acc0; 00232 00233 d2 = (a2 * acc0) + d2; 00234 00235 /* y[n] = b0 * x[n] + d1 */ 00236 acc1 = (b0 * Xn2) + d1; 00237 00238 d1 = (b1 * Xn2) + d2; 00239 00240 d2 = (b2 * Xn2); 00241 00242 /* Store the result in the accumulator in the destination buffer. */ 00243 *pOut++ = acc1; 00244 00245 d1 = (a1 * acc1) + d1; 00246 00247 d2 = (a2 * acc1) + d2; 00248 00249 /* decrement the loop counter */ 00250 sample--; 00251 00252 } 00253 00254 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00255 ** No loop unrolling is used. */ 00256 sample = blockSize & 0x3u; 00257 00258 while(sample > 0u) 00259 { 00260 /* Read the input */ 00261 Xn = *pIn++; 00262 00263 /* y[n] = b0 * x[n] + d1 */ 00264 acc0 = (b0 * Xn) + d1; 00265 00266 /* Store the result in the accumulator in the destination buffer. */ 00267 *pOut++ = acc0; 00268 00269 /* Every time after the output is computed state should be updated. */ 00270 /* d1 = b1 * x[n] + a1 * y[n] + d2 */ 00271 d1 = ((b1 * Xn) + (a1 * acc0)) + d2; 00272 00273 /* d2 = b2 * x[n] + a2 * y[n] */ 00274 d2 = (b2 * Xn) + (a2 * acc0); 00275 00276 /* decrement the loop counter */ 00277 sample--; 00278 } 00279 00280 /* Store the updated state variables back into the state array */ 00281 *pState++ = d1; 00282 *pState++ = d2; 00283 00284 /* The current stage input is given as the output to the next stage */ 00285 pIn = pDst; 00286 00287 /*Reset the output working pointer */ 00288 pOut = pDst; 00289 00290 /* decrement the loop counter */ 00291 stage--; 00292 00293 } while(stage > 0u); 00294 00295 } 00296 00297
