CMSIS DSP Library from CMSIS 2.0. See http://www.onarm.com/cmsis/ for full details
Dependents: K22F_DSP_Matrix_least_square BNO055-ELEC3810 1BNO055 ECE4180Project--Slave2 ... more
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 */
Generated on Tue Jul 12 2022 14:13:53 by 1.7.2