00001 /* ---------------------------------------------------------------------- 00002 * Copyright (C) 2010 ARM Limited. All rights reserved. 00003 * 00004 * $Date: 29. November 2010 00005 * $Revision: V1.0.3 00006 * 00007 * Project: CMSIS 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-M4/Cortex-M3 00014 * 00015 * Version 1.0.3 2010/11/29 00016 * Re-organized the CMSIS folders and updated documentation. 00017 * 00018 * Version 1.0.2 2010/11/11 00019 * Documentation updated. 00020 * 00021 * Version 1.0.1 2010/10/05 00022 * Production release and review comments incorporated. 00023 * 00024 * Version 1.0.0 2010/09/20 00025 * Production release and review comments incorporated. 00026 * 00027 * Version 0.0.5 2010/04/26 00028 * incorporated review comments and updated with latest CMSIS layer 00029 * 00030 * Version 0.0.3 2010/03/10 00031 * Initial version 00032 * -------------------------------------------------------------------- */ 00033 00034 #include "arm_math.h" 00035 00167 void arm_biquad_cascade_df1_f32( 00168 const arm_biquad_casd_df1_inst_f32 * S, 00169 float32_t * pSrc, 00170 float32_t * pDst, 00171 uint32_t blockSize) 00172 { 00173 float32_t *pIn = pSrc; /* source pointer */ 00174 float32_t *pOut = pDst; /* destination pointer */ 00175 float32_t *pState = S->pState; /* pState pointer */ 00176 float32_t *pCoeffs = S->pCoeffs; /* coefficient pointer */ 00177 float32_t acc; /* Simulates the accumulator */ 00178 float32_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00179 float32_t Xn1, Xn2, Yn1, Yn2; /* Filter pState variables */ 00180 float32_t Xn; /* temporary input */ 00181 uint32_t sample, stage = S->numStages; /* loop counters */ 00182 00183 00184 do 00185 { 00186 /* Reading the coefficients */ 00187 b0 = *pCoeffs++; 00188 b1 = *pCoeffs++; 00189 b2 = *pCoeffs++; 00190 a1 = *pCoeffs++; 00191 a2 = *pCoeffs++; 00192 00193 /* Reading the pState values */ 00194 Xn1 = pState[0]; 00195 Xn2 = pState[1]; 00196 Yn1 = pState[2]; 00197 Yn2 = pState[3]; 00198 00199 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00200 /* The variable acc hold output values that are being computed: 00201 * 00202 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00203 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00204 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00205 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00206 */ 00207 00208 sample = blockSize >> 2u; 00209 00210 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00211 ** a second loop below computes the remaining 1 to 3 samples. */ 00212 while(sample > 0u) 00213 { 00214 /* Read the first input */ 00215 Xn = *pIn++; 00216 00217 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00218 Yn2 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00219 00220 /* Store the result in the accumulator in the destination buffer. */ 00221 *pOut++ = Yn2; 00222 00223 /* Every time after the output is computed state should be updated. */ 00224 /* The states should be updated as: */ 00225 /* Xn2 = Xn1 */ 00226 /* Xn1 = Xn */ 00227 /* Yn2 = Yn1 */ 00228 /* Yn1 = acc */ 00229 00230 /* Read the second input */ 00231 Xn2 = *pIn++; 00232 00233 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00234 Yn1 = (b0 * Xn2) + (b1 * Xn) + (b2 * Xn1) + (a1 * Yn2) + (a2 * Yn1); 00235 00236 /* Store the result in the accumulator in the destination buffer. */ 00237 *pOut++ = Yn1; 00238 00239 /* Every time after the output is computed state should be updated. */ 00240 /* The states should be updated as: */ 00241 /* Xn2 = Xn1 */ 00242 /* Xn1 = Xn */ 00243 /* Yn2 = Yn1 */ 00244 /* Yn1 = acc */ 00245 00246 /* Read the third input */ 00247 Xn1 = *pIn++; 00248 00249 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00250 Yn2 = (b0 * Xn1) + (b1 * Xn2) + (b2 * Xn) + (a1 * Yn1) + (a2 * Yn2); 00251 00252 /* Store the result in the accumulator in the destination buffer. */ 00253 *pOut++ = Yn2; 00254 00255 /* Every time after the output is computed state should be updated. */ 00256 /* The states should be updated as: */ 00257 /* Xn2 = Xn1 */ 00258 /* Xn1 = Xn */ 00259 /* Yn2 = Yn1 */ 00260 /* Yn1 = acc */ 00261 00262 /* Read the forth input */ 00263 Xn = *pIn++; 00264 00265 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00266 Yn1 = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn2) + (a2 * Yn1); 00267 00268 /* Store the result in the accumulator in the destination buffer. */ 00269 *pOut++ = Yn1; 00270 00271 /* Every time after the output is computed state should be updated. */ 00272 /* The states should be updated as: */ 00273 /* Xn2 = Xn1 */ 00274 /* Xn1 = Xn */ 00275 /* Yn2 = Yn1 */ 00276 /* Yn1 = acc */ 00277 Xn2 = Xn1; 00278 Xn1 = Xn; 00279 00280 /* decrement the loop counter */ 00281 sample--; 00282 00283 } 00284 00285 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00286 ** No loop unrolling is used. */ 00287 sample = blockSize & 0x3u; 00288 00289 while(sample > 0u) 00290 { 00291 /* Read the input */ 00292 Xn = *pIn++; 00293 00294 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00295 acc = (b0 * Xn) + (b1 * Xn1) + (b2 * Xn2) + (a1 * Yn1) + (a2 * Yn2); 00296 00297 /* Store the result in the accumulator in the destination buffer. */ 00298 *pOut++ = acc; 00299 00300 /* Every time after the output is computed state should be updated. */ 00301 /* The states should be updated as: */ 00302 /* Xn2 = Xn1 */ 00303 /* Xn1 = Xn */ 00304 /* Yn2 = Yn1 */ 00305 /* Yn1 = acc */ 00306 Xn2 = Xn1; 00307 Xn1 = Xn; 00308 Yn2 = Yn1; 00309 Yn1 = acc; 00310 00311 /* decrement the loop counter */ 00312 sample--; 00313 00314 } 00315 00316 /* Store the updated state variables back into the pState array */ 00317 *pState++ = Xn1; 00318 *pState++ = Xn2; 00319 *pState++ = Yn1; 00320 *pState++ = Yn2; 00321 00322 /* The first stage goes from the input wire to the output wire. */ 00323 /* Subsequent numStages occur in-place in the output wire */ 00324 pIn = pDst; 00325 00326 /* Reset the output pointer */ 00327 pOut = pDst; 00328 00329 /* decrement the loop counter */ 00330 stage--; 00331 00332 } while(stage > 0u); 00333 00334 } 00335 00336