git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_iir_lattice_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_iir_lattice_f32.c
   9 *
  10 * Description:  Floating-point IIR Lattice filter processing function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Version 1.0.10 2011/7/15
  15 *    Big Endian support added and Merged M0 and M3/M4 Source code.
  16 *
  17 * Version 1.0.3 2010/11/29
  18 *    Re-organized the CMSIS folders and updated documentation.
  19 *
  20 * Version 1.0.2 2010/11/11
  21 *    Documentation updated.
  22 *
  23 * Version 1.0.1 2010/10/05
  24 *    Production release and review comments incorporated.
  25 *
  26 * Version 1.0.0 2010/09/20
  27 *    Production release and review comments incorporated
  28 *
  29 * Version 0.0.7  2010/06/10
  30 *    Misra-C changes done
  31 * -------------------------------------------------------------------- */
  32
  33 #include "arm_math.h"
  34
  35 /**
  36  * @ingroup groupFilters
  37  */
  38
  39 /**
  40  * @defgroup IIR_Lattice Infinite Impulse Response (IIR) Lattice Filters
  41  *
  42  * This set of functions implements lattice filters
  43  * for Q15, Q31 and floating-point data types.  Lattice filters are used in a
  44  * variety of adaptive filter applications.  The filter structure has feedforward and
  45  * feedback components and the net impulse response is infinite length.
  46  * The functions operate on blocks
  47  * of input and output data and each call to the function processes
  48  * <code>blockSize</code> samples through the filter.  <code>pSrc</code> and
  49  * <code>pDst</code> point to input and output arrays containing <code>blockSize</code> values.
  50
  51  * \par Algorithm:
  52  * \image html IIRLattice.gif "Infinite Impulse Response Lattice filter"
  53  * <pre>
  54  *    fN(n)   =  x(n)
  55  *    fm-1(n) = fm(n) - km * gm-1(n-1)   for m = N, N-1, ...1
  56  *    gm(n)   = km * fm-1(n) + gm-1(n-1) for m = N, N-1, ...1
  57  *    y(n)    = vN * gN(n) + vN-1 * gN-1(n) + ...+ v0 * g0(n)
  58  * </pre>
  59  * \par
  60  * <code>pkCoeffs</code> points to array of reflection coefficients of size <code>numStages</code>.
  61  * Reflection coefficients are stored in time-reversed order.
  62  * \par
  63  * <pre>
  64  *    {kN, kN-1, ....k1}
  65  * </pre>
  66  * <code>pvCoeffs</code> points to the array of ladder coefficients of size <code>(numStages+1)</code>.
  67  * Ladder coefficients are stored in time-reversed order.
  68  * \par
  69  * <pre>
  70  *    {vN, vN-1, ...v0}
  71  * </pre>
  72  * <code>pState</code> points to a state array of size <code>numStages + blockSize</code>.
  73  * The state variables shown in the figure above (the g values) are stored in the <code>pState</code> array.
  74  * The state variables are updated after each block of data is processed; the coefficients are untouched.
  75  * \par Instance Structure
  76  * The coefficients and state variables for a filter are stored together in an instance data structure.
  77  * A separate instance structure must be defined for each filter.
  78  * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
  79  * There are separate instance structure declarations for each of the 3 supported data types.
  80   *
  81  * \par Initialization Functions
  82  * There is also an associated initialization function for each data type.
  83  * The initialization function performs the following operations:
  84  * - Sets the values of the internal structure fields.
  85  * - Zeros out the values in the state buffer.
  86  *
  87  * \par
  88  * Use of the initialization function is optional.
  89  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
  90  * To place an instance structure into a const data section, the instance structure must be manually initialized.
  91  * Set the values in the state buffer to zeros and then manually initialize the instance structure as follows:
  92  * <pre>
  93  *arm_iir_lattice_instance_f32 S = {numStages, pState, pkCoeffs, pvCoeffs};
  94  *arm_iir_lattice_instance_q31 S = {numStages, pState, pkCoeffs, pvCoeffs};
  95  *arm_iir_lattice_instance_q15 S = {numStages, pState, pkCoeffs, pvCoeffs};
  96  * </pre>
  97  * \par
  98  * where <code>numStages</code> is the number of stages in the filter; <code>pState</code> points to the state buffer array;
  99  * <code>pkCoeffs</code> points to array of the reflection coefficients; <code>pvCoeffs</code> points to the array of ladder coefficients.
 100  * \par Fixed-Point Behavior
 101  * Care must be taken when using the fixed-point versions of the IIR lattice filter functions.
 102  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
 103  * Refer to the function specific documentation below for usage guidelines.
 104  */
 105
 106 /**
 107  * @addtogroup IIR_Lattice
 108  * @{
 109  */
 110
 111 /**
 112  * @brief Processing function for the floating-point IIR lattice filter.
 113  * @param[in] *S points to an instance of the floating-point IIR lattice structure.
 114  * @param[in] *pSrc points to the block of input data.
 115  * @param[out] *pDst points to the block of output data.
 116  * @param[in] blockSize number of samples to process.
 117  * @return none.
 118  */
 119
 120 void arm_iir_lattice_f32(
 121   const arm_iir_lattice_instance_f32 * S,
 122   float32_t * pSrc,
 123   float32_t * pDst,
 124   uint32_t blockSize)
 125 {
 126   float32_t fcurr, fnext = 0, gcurr, gnext;      /* Temporary variables for lattice stages */
 127   float32_t acc;                                 /* Accumlator */
 128   uint32_t blkCnt, tapCnt;                       /* temporary variables for counts */
 129   float32_t *px1, *px2, *pk, *pv;                /* temporary pointers for state and coef */
 130   uint32_t numStages = S->numStages;             /* number of stages */
 131   float32_t *pState;                             /* State pointer */
 132   float32_t *pStateCurnt;                        /* State current pointer */
 133
 134
 135 #ifndef ARM_MATH_CM0
 136
 137   /* Run the below code for Cortex-M4 and Cortex-M3 */
 138
 139   gcurr = 0.0f;
 140   blkCnt = blockSize;
 141
 142   pState = &S->pState[0];
 143
 144   /* Sample processing */
 145   while(blkCnt > 0u)
 146   {
 147     /* Read Sample from input buffer */
 148     /* fN(n) = x(n) */
 149     fcurr = *pSrc++;
 150
 151     /* Initialize state read pointer */
 152     px1 = pState;
 153     /* Initialize state write pointer */
 154     px2 = pState;
 155     /* Set accumulator to zero */
 156     acc = 0.0f;
 157     /* Initialize Ladder coeff pointer */
 158     pv = &S->pvCoeffs[0];
 159     /* Initialize Reflection coeff pointer */
 160     pk = &S->pkCoeffs[0];
 161
 162
 163     /* Process sample for first tap */
 164     gcurr = *px1++;
 165     /* fN-1(n) = fN(n) - kN * gN-1(n-1) */
 166     fnext = fcurr - ((*pk) * gcurr);
 167     /* gN(n) = kN * fN-1(n) + gN-1(n-1) */
 168     gnext = (fnext * (*pk++)) + gcurr;
 169     /* write gN(n) into state for next sample processing */
 170     *px2++ = gnext;
 171     /* y(n) += gN(n) * vN  */
 172     acc += (gnext * (*pv++));
 173
 174     /* Update f values for next coefficient processing */
 175     fcurr = fnext;
 176
 177     /* Loop unrolling.  Process 4 taps at a time. */
 178     tapCnt = (numStages - 1u) >> 2;
 179
 180     while(tapCnt > 0u)
 181     {
 182       /* Process sample for 2nd, 6th ...taps */
 183       /* Read gN-2(n-1) from state buffer */
 184       gcurr = *px1++;
 185       /* Process sample for 2nd, 6th .. taps */
 186       /* fN-2(n) = fN-1(n) - kN-1 * gN-2(n-1) */
 187       fnext = fcurr - ((*pk) * gcurr);
 188       /* gN-1(n) = kN-1 * fN-2(n) + gN-2(n-1) */
 189       gnext = (fnext * (*pk++)) + gcurr;
 190       /* y(n) += gN-1(n) * vN-1  */
 191       /* process for gN-5(n) * vN-5, gN-9(n) * vN-9 ... */
 192       acc += (gnext * (*pv++));
 193       /* write gN-1(n) into state for next sample processing */
 194       *px2++ = gnext;
 195
 196
 197       /* Process sample for 3nd, 7th ...taps */
 198       /* Read gN-3(n-1) from state buffer */
 199       gcurr = *px1++;
 200       /* Process sample for 3rd, 7th .. taps */
 201       /* fN-3(n) = fN-2(n) - kN-2 * gN-3(n-1) */
 202       fcurr = fnext - ((*pk) * gcurr);
 203       /* gN-2(n) = kN-2 * fN-3(n) + gN-3(n-1) */
 204       gnext = (fcurr * (*pk++)) + gcurr;
 205       /* y(n) += gN-2(n) * vN-2  */
 206       /* process for gN-6(n) * vN-6, gN-10(n) * vN-10 ... */
 207       acc += (gnext * (*pv++));
 208       /* write gN-2(n) into state for next sample processing */
 209       *px2++ = gnext;
 210
 211
 212       /* Process sample for 4th, 8th ...taps */
 213       /* Read gN-4(n-1) from state buffer */
 214       gcurr = *px1++;
 215       /* Process sample for 4th, 8th .. taps */
 216       /* fN-4(n) = fN-3(n) - kN-3 * gN-4(n-1) */
 217       fnext = fcurr - ((*pk) * gcurr);
 218       /* gN-3(n) = kN-3 * fN-4(n) + gN-4(n-1) */
 219       gnext = (fnext * (*pk++)) + gcurr;
 220       /* y(n) += gN-3(n) * vN-3  */
 221       /* process for gN-7(n) * vN-7, gN-11(n) * vN-11 ... */
 222       acc += (gnext * (*pv++));
 223       /* write gN-3(n) into state for next sample processing */
 224       *px2++ = gnext;
 225
 226
 227       /* Process sample for 5th, 9th ...taps */
 228       /* Read gN-5(n-1) from state buffer */
 229       gcurr = *px1++;
 230       /* Process sample for 5th, 9th .. taps */
 231       /* fN-5(n) = fN-4(n) - kN-4 * gN-1(n-1) */
 232       fcurr = fnext - ((*pk) * gcurr);
 233       /* gN-4(n) = kN-4 * fN-5(n) + gN-5(n-1) */
 234       gnext = (fcurr * (*pk++)) + gcurr;
 235       /* y(n) += gN-4(n) * vN-4  */
 236       /* process for gN-8(n) * vN-8, gN-12(n) * vN-12 ... */
 237       acc += (gnext * (*pv++));
 238       /* write gN-4(n) into state for next sample processing */
 239       *px2++ = gnext;
 240
 241       tapCnt--;
 242
 243     }
 244
 245     fnext = fcurr;
 246
 247     /* If the filter length is not a multiple of 4, compute the remaining filter taps */
 248     tapCnt = (numStages - 1u) % 0x4u;
 249
 250     while(tapCnt > 0u)
 251     {
 252       gcurr = *px1++;
 253       /* Process sample for last taps */
 254       fnext = fcurr - ((*pk) * gcurr);
 255       gnext = (fnext * (*pk++)) + gcurr;
 256       /* Output samples for last taps */
 257       acc += (gnext * (*pv++));
 258       *px2++ = gnext;
 259       fcurr = fnext;
 260
 261       tapCnt--;
 262
 263     }
 264
 265
 266     /* y(n) += g0(n) * v0 */
 267     acc += (fnext * (*pv));
 268
 269     *px2++ = fnext;
 270
 271     /* write out into pDst */
 272     *pDst++ = acc;
 273
 274     /* Advance the state pointer by 4 to process the next group of 4 samples */
 275     pState = pState + 1u;
 276     blkCnt--;
 277
 278   }
 279
 280   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
 281      for the preperation of next frame process */
 282
 283   /* Points to the start of the state buffer */
 284   pStateCurnt = &S->pState[0];
 285   pState = &S->pState[blockSize];
 286
 287   tapCnt = numStages >> 2u;
 288
 289   /* copy data */
 290   while(tapCnt > 0u)
 291   {
 292     *pStateCurnt++ = *pState++;
 293     *pStateCurnt++ = *pState++;
 294     *pStateCurnt++ = *pState++;
 295     *pStateCurnt++ = *pState++;
 296
 297     /* Decrement the loop counter */
 298     tapCnt--;
 299
 300   }
 301
 302   /* Calculate remaining number of copies */
 303   tapCnt = (numStages) % 0x4u;
 304
 305   /* Copy the remaining q31_t data */
 306   while(tapCnt > 0u)
 307   {
 308     *pStateCurnt++ = *pState++;
 309
 310     /* Decrement the loop counter */
 311     tapCnt--;
 312   }
 313
 314 #else
 315
 316   /* Run the below code for Cortex-M0 */
 317
 318   blkCnt = blockSize;
 319
 320   pState = &S->pState[0];
 321
 322   /* Sample processing */
 323   while(blkCnt > 0u)
 324   {
 325     /* Read Sample from input buffer */
 326     /* fN(n) = x(n) */
 327     fcurr = *pSrc++;
 328
 329     /* Initialize state read pointer */
 330     px1 = pState;
 331     /* Initialize state write pointer */
 332     px2 = pState;
 333     /* Set accumulator to zero */
 334     acc = 0.0f;
 335     /* Initialize Ladder coeff pointer */
 336     pv = &S->pvCoeffs[0];
 337     /* Initialize Reflection coeff pointer */
 338     pk = &S->pkCoeffs[0];
 339
 340
 341     /* Process sample for numStages */
 342     tapCnt = numStages;
 343
 344     while(tapCnt > 0u)
 345     {
 346       gcurr = *px1++;
 347       /* Process sample for last taps */
 348       fnext = fcurr - ((*pk) * gcurr);
 349       gnext = (fnext * (*pk++)) + gcurr;
 350
 351       /* Output samples for last taps */
 352       acc += (gnext * (*pv++));
 353       *px2++ = gnext;
 354       fcurr = fnext;
 355
 356       /* Decrementing loop counter */
 357       tapCnt--;
 358
 359     }
 360
 361     /* y(n) += g0(n) * v0 */
 362     acc += (fnext * (*pv));
 363
 364     *px2++ = fnext;
 365
 366     /* write out into pDst */
 367     *pDst++ = acc;
 368
 369     /* Advance the state pointer by 1 to process the next group of samples */
 370     pState = pState + 1u;
 371     blkCnt--;
 372
 373   }
 374
 375   /* Processing is complete. Now copy last S->numStages samples to start of the buffer
 376      for the preperation of next frame process */
 377
 378   /* Points to the start of the state buffer */
 379   pStateCurnt = &S->pState[0];
 380   pState = &S->pState[blockSize];
 381
 382   tapCnt = numStages;
 383
 384   /* Copy the data */
 385   while(tapCnt > 0u)
 386   {
 387     *pStateCurnt++ = *pState++;
 388
 389     /* Decrement the loop counter */
 390     tapCnt--;
 391   }
 392
 393 #endif /*   #ifndef ARM_MATH_CM0 */
 394
 395 }
 396
 397
 398
 399
 400 /**
 401  * @} end of IIR_Lattice group
 402  */