CMSIS DSP Library from CMSIS 2.0. See http://www.onarm.com/cmsis/ for full details
Dependents: K22F_DSP_Matrix_least_square BNO055-ELEC3810 1BNO055 ECE4180Project--Slave2 ... more
arm_biquad_cascade_df1_f32.c
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 00036 /** 00037 * @ingroup groupFilters 00038 */ 00039 00040 /** 00041 * @defgroup BiquadCascadeDF1 Biquad Cascade IIR Filters Using Direct Form I Structure 00042 * 00043 * This set of functions implements arbitrary order recursive (IIR) filters. 00044 * The filters are implemented as a cascade of second order Biquad sections. 00045 * The functions support Q15, Q31 and floating-point data types. Fast version of Q15 and Q31 also supported. 00046 * 00047 * The functions operate on blocks of input and output data and each call to the function 00048 * processes <code>blockSize</code> samples through the filter. 00049 * <code>pSrc</code> points to the array of input data and 00050 * <code>pDst</code> points to the array of output data. 00051 * Both arrays contain <code>blockSize</code> values. 00052 * 00053 * \par Algorithm 00054 * Each Biquad stage implements a second order filter using the difference equation: 00055 * <pre> 00056 * y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] 00057 * </pre> 00058 * A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage. 00059 * \image html Biquad.gif "Single Biquad filter stage" 00060 * Coefficients <code>b0, b1 and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients. 00061 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients. 00062 * Pay careful attention to the sign of the feedback coefficients. 00063 * Some design tools use the difference equation 00064 * <pre> 00065 * y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2] 00066 * </pre> 00067 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library. 00068 * 00069 * \par 00070 * Higher order filters are realized as a cascade of second order sections. 00071 * <code>numStages</code> refers to the number of second order stages used. 00072 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages. 00073 * \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages" 00074 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>). 00075 * 00076 * \par 00077 * The <code>pState</code> points to state variables array. 00078 * Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code>. 00079 * The state variables are arranged in the <code>pState</code> array as: 00080 * <pre> 00081 * {x[n-1], x[n-2], y[n-1], y[n-2]} 00082 * </pre> 00083 * 00084 * \par 00085 * The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on. 00086 * The state array has a total length of <code>4*numStages</code> values. 00087 * The state variables are updated after each block of data is processed, the coefficients are untouched. 00088 * 00089 * \par Instance Structure 00090 * The coefficients and state variables for a filter are stored together in an instance data structure. 00091 * A separate instance structure must be defined for each filter. 00092 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared. 00093 * There are separate instance structure declarations for each of the 3 supported data types. 00094 * 00095 * \par Init Functions 00096 * There is also an associated initialization function for each data type. 00097 * The initialization function performs following operations: 00098 * - Sets the values of the internal structure fields. 00099 * - Zeros out the values in the state buffer. 00100 * 00101 * \par 00102 * Use of the initialization function is optional. 00103 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section. 00104 * To place an instance structure into a const data section, the instance structure must be manually initialized. 00105 * Set the values in the state buffer to zeros before static initialization. 00106 * The code below statically initializes each of the 3 different data type filter instance structures 00107 * <pre> 00108 * arm_biquad_casd_df1_inst_f32 S1 = {numStages, pState, pCoeffs}; 00109 * arm_biquad_casd_df1_inst_q15 S2 = {numStages, pState, pCoeffs, postShift}; 00110 * arm_biquad_casd_df1_inst_q31 S3 = {numStages, pState, pCoeffs, postShift}; 00111 * </pre> 00112 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer; 00113 * <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied. 00114 * 00115 * \par Fixed-Point Behavior 00116 * Care must be taken when using the Q15 and Q31 versions of the Biquad Cascade filter functions. 00117 * Following issues must be considered: 00118 * - Scaling of coefficients 00119 * - Filter gain 00120 * - Overflow and saturation 00121 * 00122 * \par 00123 * <b>Scaling of coefficients: </b> 00124 * Filter coefficients are represented as fractional values and 00125 * coefficients are restricted to lie in the range <code>[-1 +1)</code>. 00126 * The fixed-point functions have an additional scaling parameter <code>postShift</code> 00127 * which allow the filter coefficients to exceed the range <code>[+1 -1)</code>. 00128 * At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits. 00129 * \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator" 00130 * This essentially scales the filter coefficients by <code>2^postShift</code>. 00131 * For example, to realize the coefficients 00132 * <pre> 00133 * {1.5, -0.8, 1.2, 1.6, -0.9} 00134 * </pre> 00135 * set the pCoeffs array to: 00136 * <pre> 00137 * {0.75, -0.4, 0.6, 0.8, -0.45} 00138 * </pre> 00139 * and set <code>postShift=1</code> 00140 * 00141 * \par 00142 * <b>Filter gain: </b> 00143 * The frequency response of a Biquad filter is a function of its coefficients. 00144 * It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies. 00145 * This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter. 00146 * To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed. 00147 * 00148 * \par 00149 * <b>Overflow and saturation: </b> 00150 * For Q15 and Q31 versions, it is described separately as part of the function specific documentation below. 00151 */ 00152 00153 /** 00154 * @addtogroup BiquadCascadeDF1 00155 * @{ 00156 */ 00157 00158 /** 00159 * @param[in] *S points to an instance of the floating-point Biquad cascade structure. 00160 * @param[in] *pSrc points to the block of input data. 00161 * @param[out] *pDst points to the block of output data. 00162 * @param[in] blockSize number of samples to process per call. 00163 * @return none. 00164 * 00165 */ 00166 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 00337 /** 00338 * @} end of BiquadCascadeDF1 group 00339 */
Generated on Tue Jul 12 2022 14:13:52 by 1.7.2