arm_biquad_cascade_df2T_f32.c

00001 /* ----------------------------------------------------------------------    
00002 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.    
00003 *    
00004 * $Date:        17. January 2013  
00005 *    
00006 * Project:      CMSIS DSP Library    
00007 * Title:        arm_biquad_cascade_df2T_f32.c    
00008 *    
00009 * Description:  Processing function for the floating-point transposed    
00010 *               direct form II Biquad cascade filter.   
00011 *    
00012 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
00013 *  
00014 * Redistribution and use in source and binary forms, with or without 
00015 * modification, are permitted provided that the following conditions
00016 * are met:
00017 *   - Redistributions of source code must retain the above copyright
00018 *     notice, this list of conditions and the following disclaimer.
00019 *   - Redistributions in binary form must reproduce the above copyright
00020 *     notice, this list of conditions and the following disclaimer in
00021 *     the documentation and/or other materials provided with the 
00022 *     distribution.
00023 *   - Neither the name of ARM LIMITED nor the names of its contributors
00024 *     may be used to endorse or promote products derived from this
00025 *     software without specific prior written permission.
00026 *
00027 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00028 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00029 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00030 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00031 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00032 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00033 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00034 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00035 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00036 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00037 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00038 * POSSIBILITY OF SUCH DAMAGE.   
00039 * -------------------------------------------------------------------- */
00040 
00041 #include "arm_math.h"
00042 
00043 /**       
00044 * @ingroup groupFilters       
00045 */
00046 
00047 /**       
00048 * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure       
00049 *       
00050 * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.       
00051 * The filters are implemented as a cascade of second order Biquad sections.       
00052 * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.      
00053 * Only floating-point data is supported.       
00054 *       
00055 * This function operate on blocks of input and output data and each call to the function       
00056 * processes <code>blockSize</code> samples through the filter.       
00057 * <code>pSrc</code> points to the array of input data and       
00058 * <code>pDst</code> points to the array of output data.       
00059 * Both arrays contain <code>blockSize</code> values.       
00060 *       
00061 * \par Algorithm       
00062 * Each Biquad stage implements a second order filter using the difference equation:       
00063 * <pre>       
00064 *    y[n] = b0 * x[n] + d1       
00065 *    d1 = b1 * x[n] + a1 * y[n] + d2       
00066 *    d2 = b2 * x[n] + a2 * y[n]       
00067 * </pre>       
00068 * where d1 and d2 represent the two state values.       
00069 *       
00070 * \par       
00071 * A Biquad filter using a transposed Direct Form II structure is shown below.       
00072 * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"       
00073 * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.       
00074 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.       
00075 * Pay careful attention to the sign of the feedback coefficients.       
00076 * Some design tools flip the sign of the feedback coefficients:       
00077 * <pre>       
00078 *    y[n] = b0 * x[n] + d1;       
00079 *    d1 = b1 * x[n] - a1 * y[n] + d2;       
00080 *    d2 = b2 * x[n] - a2 * y[n];       
00081 * </pre>       
00082 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.       
00083 *       
00084 * \par       
00085 * Higher order filters are realized as a cascade of second order sections.       
00086 * <code>numStages</code> refers to the number of second order stages used.       
00087 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.       
00088 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the       
00089 * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).       
00090 *       
00091 * \par       
00092 * <code>pState</code> points to the state variable array.       
00093 * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.       
00094 * The state variables are arranged in the <code>pState</code> array as:       
00095 * <pre>       
00096 *     {d11, d12, d21, d22, ...}       
00097 * </pre>       
00098 * where <code>d1x</code> refers to the state variables for the first Biquad and       
00099 * <code>d2x</code> refers to the state variables for the second Biquad.       
00100 * The state array has a total length of <code>2*numStages</code> values.       
00101 * The state variables are updated after each block of data is processed; the coefficients are untouched.       
00102 *       
00103 * \par       
00104 * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.    
00105 * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.    
00106 * That is why the Direct Form I structure supports Q15 and Q31 data types.    
00107 * The transposed Direct Form II structure, on the other hand, requires a wide dynamic range for the state variables <code>d1</code> and <code>d2</code>.    
00108 * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.    
00109 * The advantage of the Direct Form II Biquad is that it requires half the number of state variables, 2 rather than 4, per Biquad stage.    
00110 *       
00111 * \par Instance Structure       
00112 * The coefficients and state variables for a filter are stored together in an instance data structure.       
00113 * A separate instance structure must be defined for each filter.       
00114 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.       
00115 *       
00116 * \par Init Functions       
00117 * There is also an associated initialization function.      
00118 * The initialization function performs following operations:       
00119 * - Sets the values of the internal structure fields.       
00120 * - Zeros out the values in the state buffer.       
00121 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
00122 * numStages, pCoeffs, pState. Also set all of the values in pState to zero. 
00123 *       
00124 * \par       
00125 * Use of the initialization function is optional.       
00126 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.       
00127 * To place an instance structure into a const data section, the instance structure must be manually initialized.       
00128 * Set the values in the state buffer to zeros before static initialization.       
00129 * For example, to statically initialize the instance structure use       
00130 * <pre>       
00131 *     arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};       
00132 * </pre>       
00133 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer.       
00134 * <code>pCoeffs</code> is the address of the coefficient buffer;        
00135 *       
00136 */
00137 
00138 /**       
00139 * @addtogroup BiquadCascadeDF2T       
00140 * @{       
00141 */
00142 
00143 /**      
00144 * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.      
00145 * @param[in]  *S        points to an instance of the filter data structure.      
00146 * @param[in]  *pSrc     points to the block of input data.      
00147 * @param[out] *pDst     points to the block of output data      
00148 * @param[in]  blockSize number of samples to process.      
00149 * @return none.      
00150 */
00151 
00152 
00153 LOW_OPTIMIZATION_ENTER
00154 void arm_biquad_cascade_df2T_f32(
00155 const arm_biquad_cascade_df2T_instance_f32 * S,
00156 float32_t * pSrc,
00157 float32_t * pDst,
00158 uint32_t blockSize)
00159 {
00160 
00161    float32_t *pIn = pSrc;                         /*  source pointer            */
00162    float32_t *pOut = pDst;                        /*  destination pointer       */
00163    float32_t *pState = S->pState;                 /*  State pointer             */
00164    float32_t *pCoeffs = S->pCoeffs;               /*  coefficient pointer       */
00165    float32_t acc1;                                /*  accumulator               */
00166    float32_t b0, b1, b2, a1, a2;                  /*  Filter coefficients       */
00167    float32_t Xn1;                                 /*  temporary input           */
00168    float32_t d1, d2;                              /*  state variables           */
00169    uint32_t sample, stage = S->numStages;         /*  loop counters             */
00170 
00171 #ifndef ARM_MATH_CM0_FAMILY_FAMILY
00172 
00173    float32_t Xn2, Xn3, Xn4;                       /*  Input State variables     */
00174    float32_t acc2, acc3, acc4;                        /*  accumulator               */
00175 
00176 
00177    float32_t p0, p1, p2, p3, p4, A1;
00178 
00179    /* Run the below code for Cortex-M4 and Cortex-M3 */
00180    do
00181    {
00182       /* Reading the coefficients */     
00183       b0 = *pCoeffs++;
00184       b1 = *pCoeffs++;
00185       b2 = *pCoeffs++;
00186       a1 = *pCoeffs++;
00187       a2 = *pCoeffs++;
00188       
00189 
00190       /*Reading the state values */
00191       d1 = pState[0];
00192       d2 = pState[1];
00193 
00194       /* Apply loop unrolling and compute 4 output values simultaneously. */
00195       sample = blockSize >> 2u;
00196 
00197       /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.       
00198    ** a second loop below computes the remaining 1 to 3 samples. */
00199       while(sample > 0u) {
00200 
00201          /* y[n] = b0 * x[n] + d1 */
00202          /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00203          /* d2 = b2 * x[n] + a2 * y[n] */
00204 
00205          /* Read the four inputs */
00206          Xn1 = pIn[0];
00207          Xn2 = pIn[1];
00208          Xn3 = pIn[2];
00209          Xn4 = pIn[3];
00210          pIn += 4;     
00211 
00212          p0 = b0 * Xn1; 
00213          p1 = b1 * Xn1;
00214          acc1 = p0 + d1;
00215          p0 = b0 * Xn2; 
00216          p3 = a1 * acc1;
00217          p2 = b2 * Xn1;
00218          A1 = p1 + p3;
00219          p4 = a2 * acc1;
00220          d1 = A1 + d2;
00221          d2 = p2 + p4;
00222 
00223          p1 = b1 * Xn2;
00224          acc2 = p0 + d1;
00225          p0 = b0 * Xn3;  
00226          p3 = a1 * acc2; 
00227          p2 = b2 * Xn2;                                 
00228          A1 = p1 + p3;
00229          p4 = a2 * acc2;
00230          d1 = A1 + d2;
00231          d2 = p2 + p4;
00232 
00233          p1 = b1 * Xn3;
00234          acc3 = p0 + d1;
00235          p0 = b0 * Xn4; 
00236          p3 = a1 * acc3;
00237          p2 = b2 * Xn3;
00238          A1 = p1 + p3;
00239          p4 = a2 * acc3;
00240          d1 = A1 + d2;
00241          d2 = p2 + p4;
00242 
00243          acc4 = p0 + d1;
00244          p1 = b1 * Xn4;
00245          p3 = a1 * acc4;
00246          p2 = b2 * Xn4;
00247          A1 = p1 + p3;
00248          p4 = a2 * acc4;
00249          d1 = A1 + d2;
00250          d2 = p2 + p4;
00251 
00252          pOut[0] = acc1;    
00253          pOut[1] = acc2;    
00254          pOut[2] = acc3;    
00255          pOut[3] = acc4;
00256                  pOut += 4;
00257                  
00258          sample--;         
00259       }
00260 
00261       sample = blockSize & 0x3u;
00262       while(sample > 0u) {
00263          Xn1 = *pIn++;
00264 
00265          p0 = b0 * Xn1; 
00266          p1 = b1 * Xn1;
00267          acc1 = p0 + d1;
00268          p3 = a1 * acc1;
00269          p2 = b2 * Xn1;
00270          A1 = p1 + p3;
00271          p4 = a2 * acc1;
00272          d1 = A1 + d2;
00273          d2 = p2 + p4;
00274     
00275          *pOut++ = acc1;
00276          
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 #else
00296 
00297    /* Run the below code for Cortex-M0 */
00298 
00299    do
00300    {
00301       /* Reading the coefficients */
00302       b0 = *pCoeffs++;
00303       b1 = *pCoeffs++;
00304       b2 = *pCoeffs++;
00305       a1 = *pCoeffs++;
00306       a2 = *pCoeffs++;
00307 
00308       /*Reading the state values */
00309       d1 = pState[0];
00310       d2 = pState[1];
00311 
00312 
00313       sample = blockSize;
00314 
00315       while(sample > 0u)
00316       {
00317          /* Read the input */
00318          Xn1 = *pIn++;
00319 
00320          /* y[n] = b0 * x[n] + d1 */
00321          acc1 = (b0 * Xn1) + d1;
00322 
00323          /* Store the result in the accumulator in the destination buffer. */
00324          *pOut++ = acc1;
00325 
00326          /* Every time after the output is computed state should be updated. */
00327          /* d1 = b1 * x[n] + a1 * y[n] + d2 */
00328          d1 = ((b1 * Xn1) + (a1 * acc1)) + d2;
00329 
00330          /* d2 = b2 * x[n] + a2 * y[n] */
00331          d2 = (b2 * Xn1) + (a2 * acc1);
00332 
00333          /* decrement the loop counter */
00334          sample--;
00335       }
00336 
00337       /* Store the updated state variables back into the state array */
00338       *pState++ = d1;
00339       *pState++ = d2;
00340 
00341       /* The current stage input is given as the output to the next stage */
00342       pIn = pDst;
00343 
00344       /*Reset the output working pointer */
00345       pOut = pDst;
00346 
00347       /* decrement the loop counter */
00348       stage--;
00349 
00350    } while(stage > 0u);
00351 
00352 #endif /*  #ifndef ARM_MATH_CM0_FAMILY         */
00353 
00354 }
00355 LOW_OPTIMIZATION_EXIT
00356 
00357 /**       
00358    * @} end of BiquadCascadeDF2T group       
00359    */