arm_fir_interpolate_f32.c

00001 /* ----------------------------------------------------------------------  
00002 * Copyright (C) 2010 ARM Limited. All rights reserved.  
00003 *  
00004 * $Date:        29. November 2010  
00005 * $Revision:    V1.0.3  
00006 *  
00007 * Project:      CMSIS DSP Library  
00008 * Title:        arm_fir_interpolate_f32.c  
00009 *  
00010 * Description:  FIR interpolation for floating-point sequences.  
00011 *  
00012 * Target Processor: Cortex-M4/Cortex-M3
00013 *  
00014 * Version 1.0.3 2010/11/29 
00015 *    Re-organized the CMSIS folders and updated documentation.  
00016 *   
00017 * Version 1.0.2 2010/11/11  
00018 *    Documentation updated.   
00019 *  
00020 * Version 1.0.1 2010/10/05   
00021 *    Production release and review comments incorporated.  
00022 *  
00023 * Version 1.0.0 2010/09/20   
00024 *    Production release and review comments incorporated  
00025 *  
00026 * Version 0.0.7  2010/06/10   
00027 *    Misra-C changes done  
00028 * -------------------------------------------------------------------- */ 
00029  
00030 #include "arm_math.h" 
00031  
00032 /**  
00033  * @defgroup FIR_Interpolate Finite Impulse Response (FIR) Interpolator  
00034  *  
00035  * These functions combine an upsampler (zero stuffer) and an FIR filter.  
00036  * They are used in multirate systems for increasing the sample rate of a signal without introducing high frequency images.  
00037  * Conceptually, the functions are equivalent to the block diagram below:  
00038  * \image html FIRInterpolator.gif "Components included in the FIR Interpolator functions"  
00039  * After upsampling by a factor of <code>L</code>, the signal should be filtered by a lowpass filter with a normalized  
00040  * cutoff frequency of <code>1/L</code> in order to eliminate high frequency copies of the spectrum.  
00041  * The user of the function is responsible for providing the filter coefficients.  
00042  *  
00043  * The FIR interpolator functions provided in the CMSIS DSP Library combine the upsampler and FIR filter in an efficient manner.  
00044  * The upsampler inserts <code>L-1</code> zeros between each sample.  
00045  * Instead of multiplying by these zero values, the FIR filter is designed to skip them.  
00046  * This leads to an efficient implementation without any wasted effort.  
00047  * The functions operate on blocks of input and output data.  
00048  * <code>pSrc</code> points to an array of <code>blockSize</code> input values and  
00049  * <code>pDst</code> points to an array of <code>blockSize*L</code> output values.  
00050  *  
00051  * The library provides separate functions for Q15, Q31, and floating-point data types.  
00052  *  
00053  * \par Algorithm:  
00054  * The functions use a polyphase filter structure:  
00055  * <pre>  
00056  *    y[n] = b[0] * x[n] + b[L]   * x[n-1] + ... + b[L*(phaseLength-1)] * x[n-phaseLength+1]  
00057  *    y[n+1] = b[1] * x[n] + b[L+1] * x[n-1] + ... + b[L*(phaseLength-1)+1] * x[n-phaseLength+1]  
00058  *    ...  
00059  *    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]  
00060  * </pre>  
00061  * This approach is more efficient than straightforward upsample-then-filter algorithms.  
00062  * With this method the computation is reduced by a factor of <code>1/L</code> when compared to using a standard FIR filter.  
00063  * \par  
00064  * <code>pCoeffs</code> points to a coefficient array of size <code>numTaps</code>.  
00065  * <code>numTaps</code> must be a multiple of the interpolation factor <code>L</code> and this is checked by the  
00066  * initialization functions.  
00067  * Internally, the function divides the FIR filter's impulse response into shorter filters of length  
00068  * <code>phaseLength=numTaps/L</code>.  
00069  * Coefficients are stored in time reversed order.  
00070  * \par  
00071  * <pre>  
00072  *    {b[numTaps-1], b[numTaps-2], b[N-2], ..., b[1], b[0]}  
00073  * </pre>  
00074  * \par  
00075  * <code>pState</code> points to a state array of size <code>blockSize + phaseLength - 1</code>.  
00076  * Samples in the state buffer are stored in the order:  
00077  * \par  
00078  * <pre>  
00079  *    {x[n-phaseLength+1], x[n-phaseLength], x[n-phaseLength-1], x[n-phaseLength-2]....x[0], x[1], ..., x[blockSize-1]}  
00080  * </pre>  
00081  * The state variables are updated after each block of data is processed, the coefficients are untouched.  
00082  *  
00083  * \par Instance Structure  
00084  * The coefficients and state variables for a filter are stored together in an instance data structure.  
00085  * A separate instance structure must be defined for each filter.  
00086  * Coefficient arrays may be shared among several instances while state variable array should be allocated separately.  
00087  * There are separate instance structure declarations for each of the 3 supported data types.  
00088  *  
00089  * \par Initialization Functions  
00090  * There is also an associated initialization function for each data type.  
00091  * The initialization function performs the following operations:  
00092  * - Sets the values of the internal structure fields.  
00093  * - Zeros out the values in the state buffer.  
00094  * - Checks to make sure that the length of the filter is a multiple of the interpolation factor.  
00095  *  
00096  * \par  
00097  * Use of the initialization function is optional.  
00098  * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.  
00099  * To place an instance structure into a const data section, the instance structure must be manually initialized.  
00100  * The code below statically initializes each of the 3 different data type filter instance structures  
00101  * <pre>  
00102  * arm_fir_interpolate_instance_f32 S = {L, phaseLength, pCoeffs, pState};  
00103  * arm_fir_interpolate_instance_q31 S = {L, phaseLength, pCoeffs, pState};  
00104  * arm_fir_interpolate_instance_q15 S = {L, phaseLength, pCoeffs, pState};  
00105  * </pre>  
00106  * where <code>L</code> is the interpolation factor; <code>phaseLength=numTaps/L</code> is the  
00107  * length of each of the shorter FIR filters used internally,  
00108  * <code>pCoeffs</code> is the address of the coefficient buffer;  
00109  * <code>pState</code> is the address of the state buffer.  
00110  * Be sure to set the values in the state buffer to zeros when doing static initialization.  
00111  *  
00112  * \par Fixed-Point Behavior  
00113  * Care must be taken when using the fixed-point versions of the FIR interpolate filter functions.  
00114  * In particular, the overflow and saturation behavior of the accumulator used in each function must be considered.  
00115  * Refer to the function specific documentation below for usage guidelines.  
00116  */ 
00117  
00118 /**  
00119  * @addtogroup FIR_Interpolate  
00120  * @{  
00121  */ 
00122  
00123 /**  
00124  * @brief Processing function for the floating-point FIR interpolator.  
00125  * @param[in] *S        points to an instance of the floating-point FIR interpolator structure.  
00126  * @param[in] *pSrc     points to the block of input data.  
00127  * @param[out] *pDst    points to the block of output data.  
00128  * @param[in] blockSize number of input samples to process per call.  
00129  * @return none.  
00130  */ 
00131  
00132 void arm_fir_interpolate_f32( 
00133   const arm_fir_interpolate_instance_f32 * S, 
00134   float32_t * pSrc, 
00135   float32_t * pDst, 
00136   uint32_t blockSize) 
00137 { 
00138   float32_t *pState = S->pState;                 /* State pointer */ 
00139   float32_t *pCoeffs = S->pCoeffs;               /* Coefficient pointer */ 
00140   float32_t *pStateCurnt;                        /* Points to the current sample of the state */ 
00141   float32_t *ptr1, *ptr2;                        /* Temporary pointers for state and coefficient buffers */ 
00142   float32_t sum0;                                /* Accumulators */ 
00143   float32_t x0, c0;                              /* Temporary variables to hold state and coefficient values */ 
00144   uint32_t i, blkCnt, j;                         /* Loop counters */ 
00145   uint16_t phaseLen = S->phaseLength, tapCnt;    /* Length of each polyphase filter component */ 
00146  
00147  
00148   /* S->pState buffer contains previous frame (phaseLen - 1) samples */ 
00149   /* pStateCurnt points to the location where the new input data should be written */ 
00150   pStateCurnt = S->pState + (phaseLen - 1u); 
00151  
00152   /* Total number of intput samples */ 
00153   blkCnt = blockSize; 
00154  
00155   /* Loop over the blockSize. */ 
00156   while(blkCnt > 0u) 
00157   { 
00158     /* Copy new input sample into the state buffer */ 
00159     *pStateCurnt++ = *pSrc++; 
00160  
00161     /* Address modifier index of coefficient buffer */ 
00162     j = 1u; 
00163  
00164     /* Loop over the Interpolation factor. */ 
00165     i = S->L; 
00166     while(i > 0u) 
00167     { 
00168       /* Set accumulator to zero */ 
00169       sum0 = 0.0f; 
00170  
00171       /* Initialize state pointer */ 
00172       ptr1 = pState; 
00173  
00174       /* Initialize coefficient pointer */ 
00175       ptr2 = pCoeffs + (S->L - j); 
00176  
00177       /* Loop over the polyPhase length. Unroll by a factor of 4.  
00178        ** Repeat until we've computed numTaps-(4*S->L) coefficients. */ 
00179       tapCnt = phaseLen >> 2u; 
00180       while(tapCnt > 0u) 
00181       { 
00182  
00183         /* Read the coefficient */ 
00184         c0 = *(ptr2); 
00185  
00186         /* Upsampling is done by stuffing L-1 zeros between each sample.  
00187          * So instead of multiplying zeros with coefficients,  
00188          * Increment the coefficient pointer by interpolation factor times. */ 
00189         ptr2 += S->L; 
00190  
00191         /* Read the input sample */ 
00192         x0 = *(ptr1++); 
00193  
00194         /* Perform the multiply-accumulate */ 
00195         sum0 += x0 * c0; 
00196  
00197         /* Read the coefficient */ 
00198         c0 = *(ptr2); 
00199  
00200         /* Increment the coefficient pointer by interpolation factor times. */ 
00201         ptr2 += S->L; 
00202  
00203         /* Read the input sample */ 
00204         x0 = *(ptr1++); 
00205  
00206         /* Perform the multiply-accumulate */ 
00207         sum0 += x0 * c0; 
00208  
00209         /* Read the coefficient */ 
00210         c0 = *(ptr2); 
00211  
00212         /* Increment the coefficient pointer by interpolation factor times. */ 
00213         ptr2 += S->L; 
00214  
00215         /* Read the input sample */ 
00216         x0 = *(ptr1++); 
00217  
00218         /* Perform the multiply-accumulate */ 
00219         sum0 += x0 * c0; 
00220  
00221         /* Read the coefficient */ 
00222         c0 = *(ptr2); 
00223  
00224         /* Increment the coefficient pointer by interpolation factor times. */ 
00225         ptr2 += S->L; 
00226  
00227         /* Read the input sample */ 
00228         x0 = *(ptr1++); 
00229  
00230         /* Perform the multiply-accumulate */ 
00231         sum0 += x0 * c0; 
00232  
00233         /* Decrement the loop counter */ 
00234         tapCnt--; 
00235       } 
00236  
00237       /* If the polyPhase length is not a multiple of 4, compute the remaining filter taps */ 
00238       tapCnt = phaseLen % 0x4u; 
00239  
00240       while(tapCnt > 0u) 
00241       { 
00242         /* Perform the multiply-accumulate */ 
00243         sum0 += *(ptr1++) * (*ptr2); 
00244  
00245         /* Increment the coefficient pointer by interpolation factor times. */ 
00246         ptr2 += S->L; 
00247  
00248         /* Decrement the loop counter */ 
00249         tapCnt--; 
00250       } 
00251  
00252       /* The result is in the accumulator, store in the destination buffer. */ 
00253       *pDst++ = sum0; 
00254  
00255       /* Increment the address modifier index of coefficient buffer */ 
00256       j++; 
00257  
00258       /* Decrement the loop counter */ 
00259       i--; 
00260     } 
00261  
00262     /* Advance the state pointer by 1  
00263      * to process the next group of interpolation factor number samples */ 
00264     pState = pState + 1; 
00265  
00266     /* Decrement the loop counter */ 
00267     blkCnt--; 
00268   } 
00269  
00270   /* Processing is complete.  
00271    ** Now copy the last phaseLen - 1 samples to the satrt of the state buffer.  
00272    ** This prepares the state buffer for the next function call. */ 
00273  
00274   /* Points to the start of the state buffer */ 
00275   pStateCurnt = S->pState; 
00276  
00277   tapCnt = (phaseLen - 1u) >> 2u; 
00278  
00279   /* copy data */ 
00280   while(tapCnt > 0u) 
00281   { 
00282     *pStateCurnt++ = *pState++; 
00283     *pStateCurnt++ = *pState++; 
00284     *pStateCurnt++ = *pState++; 
00285     *pStateCurnt++ = *pState++; 
00286  
00287     /* Decrement the loop counter */ 
00288     tapCnt--; 
00289   } 
00290  
00291   tapCnt = (phaseLen - 1u) % 0x04u; 
00292  
00293   while(tapCnt > 0u) 
00294   { 
00295     *pStateCurnt++ = *pState++; 
00296  
00297     /* Decrement the loop counter */ 
00298     tapCnt--; 
00299   } 
00300 } 
00301  
00302  /**  
00303   * @} end of FIR_Interpolate group  
00304   */