arm_biquad_cascade_df2T_f32.c

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