Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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;
00074 q15_t *pCoeffs = S->pCoeffs;
00075 q15_t *pStateCurnt;
00076 q15_t *px, *pb;
00077 q15_t mu = S->mu;
00078 uint32_t numTaps = S->numTaps;
00079 uint32_t tapCnt, blkCnt;
00080 q31_t energy;
00081 q63_t acc;
00082 q15_t e = 0, d = 0;
00083 q15_t w = 0, in;
00084 q15_t x0;
00085 uint32_t shift = (uint32_t) S->postShift + 1u;
00086 q15_t errorXmu, oneByEnergy;
00087 q15_t postShift;
00088 q31_t coef;
00089
00090 energy = S->energy;
00091 x0 = S->x0;
00092
00093
00094
00095 pStateCurnt = &(S->pState[(numTaps - 1u)]);
00096
00097 blkCnt = blockSize;
00098
00099 while(blkCnt > 0u)
00100 {
00101
00102 *pStateCurnt++ = *pSrc;
00103
00104
00105 px = pState;
00106
00107
00108 pb = (pCoeffs);
00109
00110
00111 in = *pSrc++;
00112
00113
00114 energy -= (((q31_t) x0 * (x0)) >> 15);
00115 energy += (((q31_t) in * (in)) >> 15);
00116
00117
00118 acc = 0;
00119
00120
00121 tapCnt = numTaps >> 2;
00122
00123 while(tapCnt > 0u)
00124 {
00125
00126
00127 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00128 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00129
00130
00131 tapCnt--;
00132 }
00133
00134
00135 tapCnt = numTaps % 0x4u;
00136
00137 while(tapCnt > 0u)
00138 {
00139
00140 acc += (((q31_t) * px++ * (*pb++)));
00141
00142
00143 tapCnt--;
00144 }
00145
00146
00147 acc = __SSAT((acc >> (16u - shift)), 16u);
00148
00149
00150 *pOut++ = (q15_t) acc;
00151
00152
00153 d = *pRef++;
00154 e = d - (q15_t) acc;
00155 *pErr++ = e;
00156
00157
00158 postShift = arm_recip_q15((q15_t) energy + DELTA_Q15,
00159 &oneByEnergy, S->recipTable);
00160
00161
00162 errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
00163
00164
00165 acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
00166
00167
00168 w = (q15_t) __SSAT((q31_t) acc, 16);
00169
00170
00171 px = pState;
00172
00173
00174 pb = (pCoeffs);
00175
00176
00177 tapCnt = numTaps >> 2;
00178
00179
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
00192 tapCnt--;
00193 }
00194
00195
00196 tapCnt = numTaps % 0x4u;
00197
00198 while(tapCnt > 0u)
00199 {
00200
00201 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00202 *pb++ = (q15_t) __SSAT((coef), 16);
00203
00204
00205 tapCnt--;
00206 }
00207
00208
00209 x0 = *pState;
00210
00211
00212 pState = pState + 1u;
00213
00214
00215 blkCnt--;
00216 }
00217
00218
00219 S->energy = (q15_t) energy;
00220 S->x0 = x0;
00221
00222
00223
00224
00225
00226
00227 pStateCurnt = S->pState;
00228
00229
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
00243 tapCnt = (numTaps - 1u) % 0x4u;
00244
00245
00246 while(tapCnt > 0u)
00247 {
00248 *pStateCurnt++ = *pState++;
00249
00250
00251 tapCnt--;
00252 }
00253
00254
00255 }
00256
00257