git.gag.com Git - fw/stlink/blob - exampleF4/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_fir_interpolate_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_fir_interpolate_f32.c
   9 *
  10 * Description:  FIR interpolation for floating-point sequences.
  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  * @defgroup FIR_Interpolate Finite Impulse Response (FIR) Interpolator
  37  *
  38  * These functions combine an upsampler (zero stuffer) and an FIR filter.
  39  * They are used in multirate systems for increasing the sample rate of a signal without introducing high frequency images.
  40  * Conceptually, the functions are equivalent to the block diagram below:
  41  * \image html FIRInterpolator.gif "Components included in the FIR Interpolator functions"
  42  * After upsampling by a factor of <code>L</code>, the signal should be filtered by a lowpass filter with a normalized
  43  * cutoff frequency of <code>1/L</code> in order to eliminate high frequency copies of the spectrum.
  44  * The user of the function is responsible for providing the filter coefficients.
  45  *
  46  * The FIR interpolator functions provided in the CMSIS DSP Library combine the upsampler and FIR filter in an efficient manner.
  47  * The upsampler inserts <code>L-1</code> zeros between each sample.
  48  * Instead of multiplying by these zero values, the FIR filter is designed to skip them.
  49  * This leads to an efficient implementation without any wasted effort.
  50  * The functions operate on blocks of input and output data.
  51  * <code>pSrc</code> points to an array of <code>blockSize</code> input values and
  52  * <code>pDst</code> points to an array of <code>blockSize*L</code> output values.
  53  *
  54  * The library provides separate functions for Q15, Q31, and floating-point data types.
  55  *
  56  * \par Algorithm:
  57  * The functions use a polyphase filter structure:
  58  * <pre>
  59  *    y[n] = b[0] * x[n] + b[L]   * x[n-1] + ... + b[L*(phaseLength-1)] * x[n-phaseLength+1]
  60  *    y[n+1] = b[1] * x[n] + b[L+1] * x[n-1] + ... + b[L*(phaseLength-1)+1] * x[n-phaseLength+1]
  61  *    ...
  62  *    y[n+(L-1)] = b[L-1] * x[n] + b[2*L-1] * x[n-1] + ....+ b[L*(phaseLength-1)+(L-1)] * x[n-phaseLength+1]
  63  * </pre>
  64  * This approach is more efficient than straightforward upsample-then-filter algorithms.
  65  * With this method the computation is reduced by a factor of <code>1/L</code> when compared to using a standard FIR filter.
  66  * \par
  67  * <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.
  68  * <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code> and this is checked by the
  69  * initialization functions.
  70  * Internally, the function divides the FIR filter's impulse response into shorter filters of length
  71  * <code>phaseLength=numTaps/L</code>.
  72  * Coefficients are stored in time reversed order.
  73  * \par
  74  * <pre>
  75  *    {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}
  76  * </pre>
  77  * \par
  78  * <code>pState</code> points to a state array of size <code>blockSize + phaseLength - 1</code>.
  79  * Samples in the state buffer are stored in the order:
  80  * \par
  81  * <pre>
  82  *    {x[n-phaseLength+1], x[n-phaseLength], x[n-phaseLength-1], x[n-phaseLength-2]....x[0], x[1], ..., x[blockSize-1]}
  83  * </pre>
  84  * The state variables are updated after each block of data is processed, the coefficients are untouched.
  85  *
  86  * \par Instance Structure
  87  * The coefficients and state variables for a filter are stored together in an instance data structure.
  88  * A separate instance structure must be defined for each filter.
  89  * Coefficient arrays may be shared among several instances while state variable array should be allocated separately.
  90  * There are separate instance structure declarations for each of the 3 supported data types.
  91  *
  92  * \par Initialization Functions
  93  * There is also an associated initialization function for each data type.
  94  * The initialization function performs the following operations:
  95  * - Sets the values of the internal structure fields.
  96  * - Zeros out the values in the state buffer.
  97  * - Checks to make sure that the length of the filter is a multiple of the interpolation factor.
  98  *
  99  * \par
 100  * Use of the initialization function is optional.
 101  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
 102  * To place an instance structure into a const data section, the instance structure must be manually initialized.
 103  * The code below statically initializes each of the 3 different data type filter instance structures
 104  * <pre>
 105  * arm_fir_interpolate_instance_f32 S = {L, phaseLength, pCoeffs, pState};
 106  * arm_fir_interpolate_instance_q31 S = {L, phaseLength, pCoeffs, pState};
 107  * arm_fir_interpolate_instance_q15 S = {L, phaseLength, pCoeffs, pState};
 108  * </pre>
 109  * where <code>L</code> is the interpolation factor; <code>phaseLength=numTaps/L</code> is the
 110  * length of each of the shorter FIR filters used internally,
 111  * <code>pCoeffs</code> is the address of the coefficient buffer;
 112  * <code>pState</code> is the address of the state buffer.
 113  * Be sure to set the values in the state buffer to zeros when doing static initialization.
 114  *
 115  * \par Fixed-Point Behavior
 116  * Care must be taken when using the fixed-point versions of the FIR interpolate filter functions.
 117  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.
 118  * Refer to the function specific documentation below for usage guidelines.
 119  */
 120
 121 /**
 122  * @addtogroup FIR_Interpolate
 123  * @{
 124  */
 125
 126 /**
 127  * @brief Processing function for the floating-point FIR interpolator.
 128  * @param[in] *S        points to an instance of the floating-point FIR interpolator structure.
 129  * @param[in] *pSrc     points to the block of input data.
 130  * @param[out] *pDst    points to the block of output data.
 131  * @param[in] blockSize number of input samples to process per call.
 132  * @return none.
 133  */
 134
 135 void arm_fir_interpolate_f32(
 136   const arm_fir_interpolate_instance_f32 * S,
 137   float32_t * pSrc,
 138   float32_t * pDst,
 139   uint32_t blockSize)
 140 {
 141   float32_t *pState = S->pState;                 /* State pointer */
 142   float32_t *pCoeffs = S->pCoeffs;               /* Coefficient pointer */
 143   float32_t *pStateCurnt;                        /* Points to the current sample of the state */
 144   float32_t *ptr1, *ptr2;                        /* Temporary pointers for state and coefficient buffers */
 145
 146
 147 #ifndef ARM_MATH_CM0
 148
 149   /* Run the below code for Cortex-M4 and Cortex-M3 */
 150
 151   float32_t sum0;                                /* Accumulators */
 152   float32_t x0, c0;                              /* Temporary variables to hold state and coefficient values */
 153   uint32_t i, blkCnt, j;                         /* Loop counters */
 154   uint16_t phaseLen = S->phaseLength, tapCnt;    /* Length of each polyphase filter component */
 155
 156
 157   /* S->pState buffer contains previous frame (phaseLen - 1) samples */
 158   /* pStateCurnt points to the location where the new input data should be written */
 159   pStateCurnt = S->pState + (phaseLen - 1u);
 160
 161   /* Total number of intput samples */
 162   blkCnt = blockSize;
 163
 164   /* Loop over the blockSize. */
 165   while(blkCnt > 0u)
 166   {
 167     /* Copy new input sample into the state buffer */
 168     *pStateCurnt++ = *pSrc++;
 169
 170     /* Address modifier index of coefficient buffer */
 171     j = 1u;
 172
 173     /* Loop over the Interpolation factor. */
 174     i = S->L;
 175     while(i > 0u)
 176     {
 177       /* Set accumulator to zero */
 178       sum0 = 0.0f;
 179
 180       /* Initialize state pointer */
 181       ptr1 = pState;
 182
 183       /* Initialize coefficient pointer */
 184       ptr2 = pCoeffs + (S->L - j);
 185
 186       /* Loop over the polyPhase length. Unroll by a factor of 4.
 187        ** Repeat until we've computed numTaps-(4*S->L) coefficients. */
 188       tapCnt = phaseLen >> 2u;
 189       while(tapCnt > 0u)
 190       {
 191
 192         /* Read the coefficient */
 193         c0 = *(ptr2);
 194
 195         /* Upsampling is done by stuffing L-1 zeros between each sample.
 196          * So instead of multiplying zeros with coefficients,
 197          * Increment the coefficient pointer by interpolation factor times. */
 198         ptr2 += S->L;
 199
 200         /* Read the input sample */
 201         x0 = *(ptr1++);
 202
 203         /* Perform the multiply-accumulate */
 204         sum0 += x0 * c0;
 205
 206         /* Read the coefficient */
 207         c0 = *(ptr2);
 208
 209         /* Increment the coefficient pointer by interpolation factor times. */
 210         ptr2 += S->L;
 211
 212         /* Read the input sample */
 213         x0 = *(ptr1++);
 214
 215         /* Perform the multiply-accumulate */
 216         sum0 += x0 * c0;
 217
 218         /* Read the coefficient */
 219         c0 = *(ptr2);
 220
 221         /* Increment the coefficient pointer by interpolation factor times. */
 222         ptr2 += S->L;
 223
 224         /* Read the input sample */
 225         x0 = *(ptr1++);
 226
 227         /* Perform the multiply-accumulate */
 228         sum0 += x0 * c0;
 229
 230         /* Read the coefficient */
 231         c0 = *(ptr2);
 232
 233         /* Increment the coefficient pointer by interpolation factor times. */
 234         ptr2 += S->L;
 235
 236         /* Read the input sample */
 237         x0 = *(ptr1++);
 238
 239         /* Perform the multiply-accumulate */
 240         sum0 += x0 * c0;
 241
 242         /* Decrement the loop counter */
 243         tapCnt--;
 244       }
 245
 246       /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */
 247       tapCnt = phaseLen % 0x4u;
 248
 249       while(tapCnt > 0u)
 250       {
 251         /* Perform the multiply-accumulate */
 252         sum0 += *(ptr1++) * (*ptr2);
 253
 254         /* Increment the coefficient pointer by interpolation factor times. */
 255         ptr2 += S->L;
 256
 257         /* Decrement the loop counter */
 258         tapCnt--;
 259       }
 260
 261       /* The result is in the accumulator, store in the destination buffer. */
 262       *pDst++ = sum0;
 263
 264       /* Increment the address modifier index of coefficient buffer */
 265       j++;
 266
 267       /* Decrement the loop counter */
 268       i--;
 269     }
 270
 271     /* Advance the state pointer by 1
 272      * to process the next group of interpolation factor number samples */
 273     pState = pState + 1;
 274
 275     /* Decrement the loop counter */
 276     blkCnt--;
 277   }
 278
 279   /* Processing is complete.
 280    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.
 281    ** This prepares the state buffer for the next function call. */
 282
 283   /* Points to the start of the state buffer */
 284   pStateCurnt = S->pState;
 285
 286   tapCnt = (phaseLen - 1u) >> 2u;
 287
 288   /* copy data */
 289   while(tapCnt > 0u)
 290   {
 291     *pStateCurnt++ = *pState++;
 292     *pStateCurnt++ = *pState++;
 293     *pStateCurnt++ = *pState++;
 294     *pStateCurnt++ = *pState++;
 295
 296     /* Decrement the loop counter */
 297     tapCnt--;
 298   }
 299
 300   tapCnt = (phaseLen - 1u) % 0x04u;
 301
 302   while(tapCnt > 0u)
 303   {
 304     *pStateCurnt++ = *pState++;
 305
 306     /* Decrement the loop counter */
 307     tapCnt--;
 308   }
 309
 310 #else
 311
 312   /* Run the below code for Cortex-M0 */
 313
 314   float32_t sum;                                 /* Accumulator */
 315   uint32_t i, blkCnt;                            /* Loop counters */
 316   uint16_t phaseLen = S->phaseLength, tapCnt;    /* Length of each polyphase filter component */
 317
 318
 319   /* S->pState buffer contains previous frame (phaseLen - 1) samples */
 320   /* pStateCurnt points to the location where the new input data should be written */
 321   pStateCurnt = S->pState + (phaseLen - 1u);
 322
 323   /* Total number of intput samples */
 324   blkCnt = blockSize;
 325
 326   /* Loop over the blockSize. */
 327   while(blkCnt > 0u)
 328   {
 329     /* Copy new input sample into the state buffer */
 330     *pStateCurnt++ = *pSrc++;
 331
 332     /* Loop over the Interpolation factor. */
 333     i = S->L;
 334
 335     while(i > 0u)
 336     {
 337       /* Set accumulator to zero */
 338       sum = 0.0f;
 339
 340       /* Initialize state pointer */
 341       ptr1 = pState;
 342
 343       /* Initialize coefficient pointer */
 344       ptr2 = pCoeffs + (i - 1u);
 345
 346       /* Loop over the polyPhase length */
 347       tapCnt = phaseLen;
 348
 349       while(tapCnt > 0u)
 350       {
 351         /* Perform the multiply-accumulate */
 352         sum += *ptr1++ * *ptr2;
 353
 354         /* Increment the coefficient pointer by interpolation factor times. */
 355         ptr2 += S->L;
 356
 357         /* Decrement the loop counter */
 358         tapCnt--;
 359       }
 360
 361       /* The result is in the accumulator, store in the destination buffer. */
 362       *pDst++ = sum;
 363
 364       /* Decrement the loop counter */
 365       i--;
 366     }
 367
 368     /* Advance the state pointer by 1
 369      * to process the next group of interpolation factor number samples */
 370     pState = pState + 1;
 371
 372     /* Decrement the loop counter */
 373     blkCnt--;
 374   }
 375
 376   /* Processing is complete.
 377    ** Now copy the last phaseLen - 1 samples to the start of the state buffer.
 378    ** This prepares the state buffer for the next function call. */
 379
 380   /* Points to the start of the state buffer */
 381   pStateCurnt = S->pState;
 382
 383   tapCnt = phaseLen - 1u;
 384
 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   * @} end of FIR_Interpolate group
 399   */