CMSIS DSP Software Library: arm_lms_norm_q15.c Source File
Main Page
Modules
Data Structures
Files
Examples
File List
Globals
arm_lms_norm_q15.c
Go to the documentation of this file.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_lms_norm_q15.c
00009 *
00010 * Description: Q15 NLMS filter.
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
00065 void arm_lms_norm_q15(
00066 arm_lms_norm_instance_q15 * S,
00067 q15_t * pSrc,
00068 q15_t * pRef,
00069 q15_t * pOut,
00070 q15_t * pErr,
00071 uint32_t blockSize)
00072 {
00073 q15_t *pState = S->pState; /* State pointer */
00074 q15_t *pCoeffs = S->pCoeffs; /* Coefficient pointer */
00075 q15_t *pStateCurnt; /* Points to the current sample of the state */
00076 q15_t *px, *pb; /* Temporary pointers for state and coefficient buffers */
00077 q15_t mu = S->mu; /* Adaptive factor */
00078 uint32_t numTaps = S->numTaps; /* Number of filter coefficients in the filter */
00079 uint32_t tapCnt, blkCnt; /* Loop counters */
00080 q31_t energy; /* Energy of the input */
00081 q63_t acc; /* Accumulator */
00082 q15_t e = 0, d = 0; /* error, reference data sample */
00083 q15_t w = 0, in; /* weight factor and state */
00084 q15_t x0; /* temporary variable to hold input sample */
00085 uint32_t shift = (uint32_t) S->postShift + 1u; /* Shift to be applied to the output */
00086 q15_t errorXmu, oneByEnergy; /* Temporary variables to store error and mu product and reciprocal of energy */
00087 q15_t postShift; /* Post shift to be applied to weight after reciprocal calculation */
00088 q31_t coef; /* Teporary variable for coefficient */
00089
00090 energy = S->energy;
00091 x0 = S->x0;
00092
00093 /* S->pState points to buffer which contains previous frame (numTaps - 1) samples */
00094 /* pStateCurnt points to the location where the new input data should be written */
00095 pStateCurnt = &(S->pState[(numTaps - 1u)]);
00096
00097 blkCnt = blockSize;
00098
00099 while(blkCnt > 0u)
00100 {
00101 /* Copy the new input sample into the state buffer */
00102 *pStateCurnt++ = *pSrc;
00103
00104 /* Initialize pState pointer */
00105 px = pState;
00106
00107 /* Initialize coeff pointer */
00108 pb = (pCoeffs);
00109
00110 /* Read the sample from input buffer */
00111 in = *pSrc++;
00112
00113 /* Update the energy calculation */
00114 energy -= (((q31_t) x0 * (x0)) >> 15);
00115 energy += (((q31_t) in * (in)) >> 15);
00116
00117 /* Set the accumulator to zero */
00118 acc = 0;
00119
00120 /* Loop unrolling. Process 4 taps at a time. */
00121 tapCnt = numTaps >> 2;
00122
00123 while(tapCnt > 0u)
00124 {
00125
00126 /* Perform the multiply-accumulate */
00127 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00128 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00129
00130 /* Decrement the loop counter */
00131 tapCnt--;
00132 }
00133
00134 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
00135 tapCnt = numTaps % 0x4u;
00136
00137 while(tapCnt > 0u)
00138 {
00139 /* Perform the multiply-accumulate */
00140 acc += (((q31_t) * px++ * (*pb++)));
00141
00142 /* Decrement the loop counter */
00143 tapCnt--;
00144 }
00145
00146 /* Converting the result to 1.15 format */
00147 acc = __SSAT((acc >> (16u - shift)), 16u);
00148
00149 /* Store the result from accumulator into the destination buffer. */
00150 *pOut++ = (q15_t) acc;
00151
00152 /* Compute and store error */
00153 d = *pRef++;
00154 e = d - (q15_t) acc;
00155 *pErr++ = e;
00156
00157 /* Calculation of 1/energy */
00158 postShift = arm_recip_q15((q15_t) energy + DELTA_Q15,
00159 &oneByEnergy, S->recipTable);
00160
00161 /* Calculation of e * mu value */
00162 errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
00163
00164 /* Calculation of (e * mu) * (1/energy) value */
00165 acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
00166
00167 /* Weighting factor for the normalized version */
00168 w = (q15_t) __SSAT((q31_t) acc, 16);
00169
00170 /* Initialize pState pointer */
00171 px = pState;
00172
00173 /* Initialize coeff pointer */
00174 pb = (pCoeffs);
00175
00176 /* Loop unrolling. Process 4 taps at a time. */
00177 tapCnt = numTaps >> 2;
00178
00179 /* Update filter coefficients */
00180 while(tapCnt > 0u)
00181 {
00182 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00183 *pb++ = (q15_t) __SSAT((coef), 16);
00184 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00185 *pb++ = (q15_t) __SSAT((coef), 16);
00186 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00187 *pb++ = (q15_t) __SSAT((coef), 16);
00188 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00189 *pb++ = (q15_t) __SSAT((coef), 16);
00190
00191 /* Decrement the loop counter */
00192 tapCnt--;
00193 }
00194
00195 /* If the filter length is not a multiple of 4, compute the remaining filter taps */
00196 tapCnt = numTaps % 0x4u;
00197
00198 while(tapCnt > 0u)
00199 {
00200 /* Perform the multiply-accumulate */
00201 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00202 *pb++ = (q15_t) __SSAT((coef), 16);
00203
00204 /* Decrement the loop counter */
00205 tapCnt--;
00206 }
00207
00208 /* Read the sample from state buffer */
00209 x0 = *pState;
00210
00211 /* Advance state pointer by 1 for the next sample */
00212 pState = pState + 1u;
00213
00214 /* Decrement the loop counter */
00215 blkCnt--;
00216 }
00217
00218 /* Save energy and x0 values for the next frame */
00219 S->energy = (q15_t) energy;
00220 S->x0 = x0;
00221
00222 /* Processing is complete. Now copy the last numTaps - 1 samples to the
00223 satrt of the state buffer. This prepares the state buffer for the
00224 next function call. */
00225
00226 /* Points to the start of the pState buffer */
00227 pStateCurnt = S->pState;
00228
00229 /* Calculation of count for copying integer writes */
00230 tapCnt = (numTaps - 1u) >> 2;
00231
00232 while(tapCnt > 0u)
00233 {
00234
00235 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00236 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00237
00238 tapCnt--;
00239
00240 }
00241
00242 /* Calculation of count for remaining q15_t data */
00243 tapCnt = (numTaps - 1u) % 0x4u;
00244
00245 /* copy data */
00246 while(tapCnt > 0u)
00247 {
00248 *pStateCurnt++ = *pState++;
00249
00250 /* Decrement the loop counter */
00251 tapCnt--;
00252 }
00253
00254
00255 }
00256
00257
All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Defines
Generated on Mon Nov 29 2010 17:19:57 for CMSIS DSP Software Library by
1.7.2
Wyszukiwarka
Podobne podstrony:
arm lms norm q15?arm lms init q15? sourcearm lms norm q31? sourcearm lms norm ?2? sourcearm lms norm init q15? sourcearm lms norm init ?2? sourcearm lms norm init q15?arm lms norm init q31? sourcearm mat mult q15? sourcearm correlate ?st q15? sourcearm pid init q15? sourcearm lms init q15?arm fir init q15? sourcearm lms norm ?2?arm cmplx conj q15? sourcearm mat sub q15? sourcearm mat scale q15? sourcearm q7 to q15? sourcearm pid reset q15? sourcewięcej podobnych podstron