git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df2T_f32.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        15. July 2011
   5 * $Revision:    V1.0.10
   6 *
   7 * Project:          CMSIS DSP Library
   8 * Title:            arm_biquad_cascade_df2T_f32.c
   9 *
  10 * Description:  Processing function for the floating-point transposed
  11 *               direct form II Biquad cascade filter.
  12 *
  13 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  14 *
  15 * Version 1.0.10 2011/7/15
  16 *    Big Endian support added and Merged M0 and M3/M4 Source code.
  17 *
  18 * Version 1.0.3 2010/11/29
  19 *    Re-organized the CMSIS folders and updated documentation.
  20 *
  21 * Version 1.0.2 2010/11/11
  22 *    Documentation updated.
  23 *
  24 * Version 1.0.1 2010/10/05
  25 *    Production release and review comments incorporated.
  26 *
  27 * Version 1.0.0 2010/09/20
  28 *    Production release and review comments incorporated
  29 *
  30 * Version 0.0.7  2010/06/10
  31 *    Misra-C changes done
  32 * -------------------------------------------------------------------- */
  33
  34 #include "arm_math.h"
  35
  36 /**
  37  * @ingroup groupFilters
  38  */
  39
  40 /**
  41  * @defgroup BiquadCascadeDF2T Biquad Cascade IIR Filters Using a Direct Form II Transposed Structure
  42  *
  43  * This set of functions implements arbitrary order recursive (IIR) filters using a transposed direct form II structure.
  44  * The filters are implemented as a cascade of second order Biquad sections.
  45  * These functions provide a slight memory savings as compared to the direct form I Biquad filter functions.
  46  * Only floating-point data is supported.
  47  *
  48  * This function operate on blocks of input and output data and each call to the function
  49  * processes <code>blockSize</code> samples through the filter.
  50  * <code>pSrc</code> points to the array of input data and
  51  * <code>pDst</code> points to the array of output data.
  52  * Both arrays contain <code>blockSize</code> values.
  53  *
  54  * \par Algorithm
  55  * Each Biquad stage implements a second order filter using the difference equation:
  56  * <pre>
  57  *    y[n] = b0 * x[n] + d1
  58  *    d1 = b1 * x[n] + a1 * y[n] + d2
  59  *    d2 = b2 * x[n] + a2 * y[n]
  60  * </pre>
  61  * where d1 and d2 represent the two state values.
  62  *
  63  * \par
  64  * A Biquad filter using a transposed Direct Form II structure is shown below.
  65  * \image html BiquadDF2Transposed.gif "Single transposed Direct Form II Biquad"
  66  * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
  67  * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
  68  * Pay careful attention to the sign of the feedback coefficients.
  69  * Some design tools flip the sign of the feedback coefficients:
  70  * <pre>
  71  *    y[n] = b0 * x[n] + d1;
  72  *    d1 = b1 * x[n] - a1 * y[n] + d2;
  73  *    d2 = b2 * x[n] - a2 * y[n];
  74  * </pre>
  75  * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
  76  *
  77  * \par
  78  * Higher order filters are realized as a cascade of second order sections.
  79  * <code>numStages</code> refers to the number of second order stages used.
  80  * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
  81  * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the
  82  * coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
  83  *
  84  * \par
  85  * <code>pState</code> points to the state variable array.
  86  * Each Biquad stage has 2 state variables <code>d1</code> and <code>d2</code>.
  87  * The state variables are arranged in the <code>pState</code> array as:
  88  * <pre>
  89  *     {d11, d12, d21, d22, ...}
  90  * </pre>
  91  * where <code>d1x</code> refers to the state variables for the first Biquad and
  92  * <code>d2x</code> refers to the state variables for the second Biquad.
  93  * The state array has a total length of <code>2*numStages</code> values.
  94  * The state variables are updated after each block of data is processed; the coefficients are untouched.
  95  *
  96  * \par
  97  * The CMSIS library contains Biquad filters in both Direct Form I and transposed Direct Form II.
  98  * The advantage of the Direct Form I structure is that it is numerically more robust for fixed-point data types.
  99  * That is why the Direct Form I structure supports Q15 and Q31 data types.
 100  * 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>.
 101  * Because of this, the CMSIS library only has a floating-point version of the Direct Form II Biquad.
 102  * 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.
 103  *
 104  * \par Instance Structure
 105  * The coefficients and state variables for a filter are stored together in an instance data structure.
 106  * A separate instance structure must be defined for each filter.
 107  * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
 108  *
 109  * \par Init Functions
 110  * There is also an associated initialization function.
 111  * The initialization function performs following operations:
 112  * - Sets the values of the internal structure fields.
 113  * - Zeros out the values in the state buffer.
 114  *
 115  * \par
 116  * Use of the initialization function is optional.
 117  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
 118  * To place an instance structure into a const data section, the instance structure must be manually initialized.
 119  * Set the values in the state buffer to zeros before static initialization.
 120  * For example, to statically initialize the instance structure use
 121  * <pre>
 122  *     arm_biquad_cascade_df2T_instance_f32 S1 = {numStages, pState, pCoeffs};
 123  * </pre>
 124  * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer.
 125  * <code>pCoeffs</code> is the address of the coefficient buffer;
 126  *
 127  */
 128
 129 /**
 130  * @addtogroup BiquadCascadeDF2T
 131  * @{
 132  */
 133
 134 /**
 135  * @brief Processing function for the floating-point transposed direct form II Biquad cascade filter.
 136  * @param[in]  *S        points to an instance of the filter data structure.
 137  * @param[in]  *pSrc     points to the block of input data.
 138  * @param[out] *pDst     points to the block of output data
 139  * @param[in]  blockSize number of samples to process.
 140  * @return none.
 141  */
 142
 143 void arm_biquad_cascade_df2T_f32(
 144   const arm_biquad_cascade_df2T_instance_f32 * S,
 145   float32_t * pSrc,
 146   float32_t * pDst,
 147   uint32_t blockSize)
 148 {
 149
 150   float32_t *pIn = pSrc;                         /*  source pointer            */
 151   float32_t *pOut = pDst;                        /*  destination pointer       */
 152   float32_t *pState = S->pState;                 /*  State pointer            */
 153   float32_t *pCoeffs = S->pCoeffs;               /*  coefficient pointer       */
 154   float32_t acc0;                                /*  Simulates the accumulator */
 155   float32_t b0, b1, b2, a1, a2;                  /*  Filter coefficients       */
 156   float32_t Xn;                                  /*  temporary input           */
 157   float32_t d1, d2;                              /*  state variables          */
 158   uint32_t sample, stage = S->numStages;         /*  loop counters             */
 159
 160
 161 #ifndef ARM_MATH_CM0
 162
 163   /* Run the below code for Cortex-M4 and Cortex-M3 */
 164
 165   do
 166   {
 167     /* Reading the coefficients */
 168     b0 = *pCoeffs++;
 169     b1 = *pCoeffs++;
 170     b2 = *pCoeffs++;
 171     a1 = *pCoeffs++;
 172     a2 = *pCoeffs++;
 173
 174     /*Reading the state values */
 175     d1 = pState[0];
 176     d2 = pState[1];
 177
 178     /* Apply loop unrolling and compute 4 output values simultaneously. */
 179     sample = blockSize >> 2u;
 180
 181     /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.
 182      ** a second loop below computes the remaining 1 to 3 samples. */
 183     while(sample > 0u)
 184     {
 185       /* Read the first input */
 186       Xn = *pIn++;
 187
 188       /* y[n] = b0 * x[n] + d1 */
 189       acc0 = (b0 * Xn) + d1;
 190
 191       /* Store the result in the accumulator in the destination buffer. */
 192       *pOut++ = acc0;
 193
 194       /* Every time after the output is computed state should be updated. */
 195       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 196       d1 = ((b1 * Xn) + (a1 * acc0)) + d2;
 197
 198       /* d2 = b2 * x[n] + a2 * y[n] */
 199       d2 = (b2 * Xn) + (a2 * acc0);
 200
 201       /* Read the second input */
 202       Xn = *pIn++;
 203
 204       /* y[n] = b0 * x[n] + d1 */
 205       acc0 = (b0 * Xn) + d1;
 206
 207       /* Store the result in the accumulator in the destination buffer. */
 208       *pOut++ = acc0;
 209
 210       /* Every time after the output is computed state should be updated. */
 211       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 212       d1 = ((b1 * Xn) + (a1 * acc0)) + d2;
 213
 214       /* d2 = b2 * x[n] + a2 * y[n] */
 215       d2 = (b2 * Xn) + (a2 * acc0);
 216
 217       /* Read the third input */
 218       Xn = *pIn++;
 219
 220       /* y[n] = b0 * x[n] + d1 */
 221       acc0 = (b0 * Xn) + d1;
 222
 223       /* Store the result in the accumulator in the destination buffer. */
 224       *pOut++ = acc0;
 225
 226       /* Every time after the output is computed state should be updated. */
 227       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 228       d1 = ((b1 * Xn) + (a1 * acc0)) + d2;
 229
 230       /* d2 = b2 * x[n] + a2 * y[n] */
 231       d2 = (b2 * Xn) + (a2 * acc0);
 232
 233       /* Read the fourth input */
 234       Xn = *pIn++;
 235
 236       /* y[n] = b0 * x[n] + d1 */
 237       acc0 = (b0 * Xn) + d1;
 238
 239       /* Store the result in the accumulator in the destination buffer. */
 240       *pOut++ = acc0;
 241
 242       /* Every time after the output is computed state should be updated. */
 243       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 244       d1 = (b1 * Xn) + (a1 * acc0) + d2;
 245
 246       /* d2 = b2 * x[n] + a2 * y[n] */
 247       d2 = (b2 * Xn) + (a2 * acc0);
 248
 249       /* decrement the loop counter */
 250       sample--;
 251
 252     }
 253
 254     /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
 255      ** No loop unrolling is used. */
 256     sample = blockSize & 0x3u;
 257
 258     while(sample > 0u)
 259     {
 260       /* Read the input */
 261       Xn = *pIn++;
 262
 263       /* y[n] = b0 * x[n] + d1 */
 264       acc0 = (b0 * Xn) + d1;
 265
 266       /* Store the result in the accumulator in the destination buffer. */
 267       *pOut++ = acc0;
 268
 269       /* Every time after the output is computed state should be updated. */
 270       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 271       d1 = ((b1 * Xn) + (a1 * acc0)) + d2;
 272
 273       /* d2 = b2 * x[n] + a2 * y[n] */
 274       d2 = (b2 * Xn) + (a2 * acc0);
 275
 276       /* decrement the loop counter */
 277       sample--;
 278     }
 279
 280     /* Store the updated state variables back into the state array */
 281     *pState++ = d1;
 282     *pState++ = d2;
 283
 284     /* The current stage input is given as the output to the next stage */
 285     pIn = pDst;
 286
 287     /*Reset the output working pointer */
 288     pOut = pDst;
 289
 290     /* decrement the loop counter */
 291     stage--;
 292
 293   } while(stage > 0u);
 294
 295 #else
 296
 297   /* Run the below code for Cortex-M0 */
 298
 299   do
 300   {
 301     /* Reading the coefficients */
 302     b0 = *pCoeffs++;
 303     b1 = *pCoeffs++;
 304     b2 = *pCoeffs++;
 305     a1 = *pCoeffs++;
 306     a2 = *pCoeffs++;
 307
 308     /*Reading the state values */
 309     d1 = pState[0];
 310     d2 = pState[1];
 311
 312
 313     sample = blockSize;
 314
 315     while(sample > 0u)
 316     {
 317       /* Read the input */
 318       Xn = *pIn++;
 319
 320       /* y[n] = b0 * x[n] + d1 */
 321       acc0 = (b0 * Xn) + d1;
 322
 323       /* Store the result in the accumulator in the destination buffer. */
 324       *pOut++ = acc0;
 325
 326       /* Every time after the output is computed state should be updated. */
 327       /* d1 = b1 * x[n] + a1 * y[n] + d2 */
 328       d1 = ((b1 * Xn) + (a1 * acc0)) + d2;
 329
 330       /* d2 = b2 * x[n] + a2 * y[n] */
 331       d2 = (b2 * Xn) + (a2 * acc0);
 332
 333       /* decrement the loop counter */
 334       sample--;
 335     }
 336
 337     /* Store the updated state variables back into the state array */
 338     *pState++ = d1;
 339     *pState++ = d2;
 340
 341     /* The current stage input is given as the output to the next stage */
 342     pIn = pDst;
 343
 344     /*Reset the output working pointer */
 345     pOut = pDst;
 346
 347     /* decrement the loop counter */
 348     stage--;
 349
 350   } while(stage > 0u);
 351
 352 #endif /*  #ifndef ARM_MATH_CM0         */
 353
 354 }
 355
 356
 357   /**
 358    * @} end of BiquadCascadeDF2T group
 359    */