CMSIS DSP Software Library: arm_biquad_cascade_df1_q31.c Source File Main Page Modules Data Structures Files Examples File List Globals arm_biquad_cascade_df1_q31.c Go to the documentation of this file.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_q31.c 00009 * 00010 * Description: Processing function for the 00011 * Q31 Biquad cascade 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 00066 void arm_biquad_cascade_df1_q31( 00067 const arm_biquad_casd_df1_inst_q31 * S, 00068 q31_t * pSrc, 00069 q31_t * pDst, 00070 uint32_t blockSize) 00071 { 00072 q31_t *pIn = pSrc; /* input pointer initialization */ 00073 q31_t *pOut = pDst; /* output pointer initialization */ 00074 q31_t *pState = S->pState; /* pState pointer initialization */ 00075 q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */ 00076 q63_t acc; /* accumulator */ 00077 q31_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */ 00078 q31_t b0, b1, b2, a1, a2; /* Filter coefficients */ 00079 q31_t Xn; /* temporary input */ 00080 uint32_t shift = 32u - ((uint32_t) S->postShift + 1u); /* Shift to be applied to the output */ 00081 uint32_t sample, stage = S->numStages; /* loop counters */ 00082 00083 00084 do 00085 { 00086 /* Reading the coefficients */ 00087 b0 = *pCoeffs++; 00088 b1 = *pCoeffs++; 00089 b2 = *pCoeffs++; 00090 a1 = *pCoeffs++; 00091 a2 = *pCoeffs++; 00092 00093 /* Reading the state values */ 00094 Xn1 = pState[0]; 00095 Xn2 = pState[1]; 00096 Yn1 = pState[2]; 00097 Yn2 = pState[3]; 00098 00099 /* Apply loop unrolling and compute 4 output values simultaneously. */ 00100 /* The variable acc hold output values that are being computed: 00101 * 00102 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00103 */ 00104 00105 sample = blockSize >> 2u; 00106 00107 /* First part of the processing with loop unrolling. Compute 4 outputs at a time. 00108 ** a second loop below computes the remaining 1 to 3 samples. */ 00109 while(sample > 0u) 00110 { 00111 /* Read the input */ 00112 Xn = *pIn++; 00113 00114 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00115 00116 /* acc = b0 * x[n] */ 00117 acc = (q63_t) b0 *Xn; 00118 /* acc += b1 * x[n-1] */ 00119 acc += (q63_t) b1 *Xn1; 00120 /* acc += b[2] * x[n-2] */ 00121 acc += (q63_t) b2 *Xn2; 00122 /* acc += a1 * y[n-1] */ 00123 acc += (q63_t) a1 *Yn1; 00124 /* acc += a2 * y[n-2] */ 00125 acc += (q63_t) a2 *Yn2; 00126 00127 /* The result is converted to 1.31 , Yn2 variable is reused */ 00128 Yn2 = (q31_t) (acc >> shift); 00129 00130 /* Store the output in the destination buffer. */ 00131 *pOut++ = Yn2; 00132 00133 /* Read the second input */ 00134 Xn2 = *pIn++; 00135 00136 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00137 00138 /* acc = b0 * x[n] */ 00139 acc = (q63_t) b0 *Xn2; 00140 /* acc += b1 * x[n-1] */ 00141 acc += (q63_t) b1 *Xn; 00142 /* acc += b[2] * x[n-2] */ 00143 acc += (q63_t) b2 *Xn1; 00144 /* acc += a1 * y[n-1] */ 00145 acc += (q63_t) a1 *Yn2; 00146 /* acc += a2 * y[n-2] */ 00147 acc += (q63_t) a2 *Yn1; 00148 00149 00150 /* The result is converted to 1.31, Yn1 variable is reused */ 00151 Yn1 = (q31_t) (acc >> shift); 00152 00153 /* Store the output in the destination buffer. */ 00154 *pOut++ = Yn1; 00155 00156 /* Read the third input */ 00157 Xn1 = *pIn++; 00158 00159 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00160 00161 /* acc = b0 * x[n] */ 00162 acc = (q63_t) b0 *Xn1; 00163 /* acc += b1 * x[n-1] */ 00164 acc += (q63_t) b1 *Xn2; 00165 /* acc += b[2] * x[n-2] */ 00166 acc += (q63_t) b2 *Xn; 00167 /* acc += a1 * y[n-1] */ 00168 acc += (q63_t) a1 *Yn1; 00169 /* acc += a2 * y[n-2] */ 00170 acc += (q63_t) a2 *Yn2; 00171 00172 /* The result is converted to 1.31, Yn2 variable is reused */ 00173 Yn2 = (q31_t) (acc >> shift); 00174 00175 /* Store the output in the destination buffer. */ 00176 *pOut++ = Yn2; 00177 00178 /* Read the forth input */ 00179 Xn = *pIn++; 00180 00181 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00182 00183 /* acc = b0 * x[n] */ 00184 acc = (q63_t) b0 *Xn; 00185 /* acc += b1 * x[n-1] */ 00186 acc += (q63_t) b1 *Xn1; 00187 /* acc += b[2] * x[n-2] */ 00188 acc += (q63_t) b2 *Xn2; 00189 /* acc += a1 * y[n-1] */ 00190 acc += (q63_t) a1 *Yn2; 00191 /* acc += a2 * y[n-2] */ 00192 acc += (q63_t) a2 *Yn1; 00193 00194 /* The result is converted to 1.31, Yn1 variable is reused */ 00195 Yn1 = (q31_t) (acc >> shift); 00196 00197 /* Every time after the output is computed state should be updated. */ 00198 /* The states should be updated as: */ 00199 /* Xn2 = Xn1 */ 00200 /* Xn1 = Xn */ 00201 /* Yn2 = Yn1 */ 00202 /* Yn1 = acc */ 00203 Xn2 = Xn1; 00204 Xn1 = Xn; 00205 00206 /* Store the output in the destination buffer. */ 00207 *pOut++ = Yn1; 00208 00209 /* decrement the loop counter */ 00210 sample--; 00211 } 00212 00213 /* If the blockSize is not a multiple of 4, compute any remaining output samples here. 00214 ** No loop unrolling is used. */ 00215 sample = (blockSize & 0x3u); 00216 00217 while(sample > 0u) 00218 { 00219 /* Read the input */ 00220 Xn = *pIn++; 00221 00222 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */ 00223 00224 /* acc = b0 * x[n] */ 00225 acc = (q63_t) b0 *Xn; 00226 /* acc += b1 * x[n-1] */ 00227 acc += (q63_t) b1 *Xn1; 00228 /* acc += b[2] * x[n-2] */ 00229 acc += (q63_t) b2 *Xn2; 00230 /* acc += a1 * y[n-1] */ 00231 acc += (q63_t) a1 *Yn1; 00232 /* acc += a2 * y[n-2] */ 00233 acc += (q63_t) a2 *Yn2; 00234 00235 /* The result is converted to 1.31 */ 00236 acc = acc >> shift; 00237 00238 /* Every time after the output is computed state should be updated. */ 00239 /* The states should be updated as: */ 00240 /* Xn2 = Xn1 */ 00241 /* Xn1 = Xn */ 00242 /* Yn2 = Yn1 */ 00243 /* Yn1 = acc */ 00244 Xn2 = Xn1; 00245 Xn1 = Xn; 00246 Yn2 = Yn1; 00247 Yn1 = (q31_t) acc; 00248 00249 /* Store the output in the destination buffer. */ 00250 *pOut++ = (q31_t) acc; 00251 00252 /* decrement the loop counter */ 00253 sample--; 00254 } 00255 00256 /* The first stage goes from the input buffer to the output buffer. */ 00257 /* Subsequent stages occur in-place in the output buffer */ 00258 pIn = pDst; 00259 00260 /* Reset to destination pointer */ 00261 pOut = pDst; 00262 00263 /* Store the updated state variables back into the pState array */ 00264 *pState++ = Xn1; 00265 *pState++ = Xn2; 00266 *pState++ = Yn1; 00267 *pState++ = Yn2; 00268 00269 } while(--stage); 00270 } 00271  All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines Generated on Mon Nov 29 2010 17:19:55 for CMSIS DSP Software Library by  1.7.2

Wyszukiwarka