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
00031
00032
00033 #include "arm_math.h"
00034
00071 void arm_lms_norm_q15(
00072 arm_lms_norm_instance_q15 * S,
00073 q15_t * pSrc,
00074 q15_t * pRef,
00075 q15_t * pOut,
00076 q15_t * pErr,
00077 uint32_t blockSize)
00078 {
00079 q15_t *pState = S->pState;
00080 q15_t *pCoeffs = S->pCoeffs;
00081 q15_t *pStateCurnt;
00082 q15_t *px, *pb;
00083 q15_t mu = S->mu;
00084 uint32_t numTaps = S->numTaps;
00085 uint32_t tapCnt, blkCnt;
00086 q31_t energy;
00087 q63_t acc;
00088 q15_t e = 0, d = 0;
00089 q15_t w = 0, in;
00090 q15_t x0;
00091 uint32_t shift = (uint32_t) S->postShift + 1u;
00092 q15_t errorXmu, oneByEnergy;
00093 q15_t postShift;
00094 q31_t coef;
00095
00096 energy = S->energy;
00097 x0 = S->x0;
00098
00099
00100
00101 pStateCurnt = &(S->pState[(numTaps - 1u)]);
00102
00103
00104 blkCnt = blockSize;
00105
00106
00107 #ifndef ARM_MATH_CM0
00108
00109
00110
00111 while(blkCnt > 0u)
00112 {
00113
00114 *pStateCurnt++ = *pSrc;
00115
00116
00117 px = pState;
00118
00119
00120 pb = (pCoeffs);
00121
00122
00123 in = *pSrc++;
00124
00125
00126 energy -= (((q31_t) x0 * (x0)) >> 15);
00127 energy += (((q31_t) in * (in)) >> 15);
00128
00129
00130 acc = 0;
00131
00132
00133 tapCnt = numTaps >> 2;
00134
00135 while(tapCnt > 0u)
00136 {
00137
00138
00139 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00140 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00141
00142
00143 tapCnt--;
00144 }
00145
00146
00147 tapCnt = numTaps % 0x4u;
00148
00149 while(tapCnt > 0u)
00150 {
00151
00152 acc += (((q31_t) * px++ * (*pb++)));
00153
00154
00155 tapCnt--;
00156 }
00157
00158
00159 acc = __SSAT((acc >> (16u - shift)), 16u);
00160
00161
00162 *pOut++ = (q15_t) acc;
00163
00164
00165 d = *pRef++;
00166 e = d - (q15_t) acc;
00167 *pErr++ = e;
00168
00169
00170 postShift = arm_recip_q15((q15_t) energy + DELTA_Q15,
00171 &oneByEnergy, S->recipTable);
00172
00173
00174 errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
00175
00176
00177 acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
00178
00179
00180 w = (q15_t) __SSAT((q31_t) acc, 16);
00181
00182
00183 px = pState;
00184
00185
00186 pb = (pCoeffs);
00187
00188
00189 tapCnt = numTaps >> 2;
00190
00191
00192 while(tapCnt > 0u)
00193 {
00194 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00195 *pb++ = (q15_t) __SSAT((coef), 16);
00196 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00197 *pb++ = (q15_t) __SSAT((coef), 16);
00198 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00199 *pb++ = (q15_t) __SSAT((coef), 16);
00200 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00201 *pb++ = (q15_t) __SSAT((coef), 16);
00202
00203
00204 tapCnt--;
00205 }
00206
00207
00208 tapCnt = numTaps % 0x4u;
00209
00210 while(tapCnt > 0u)
00211 {
00212
00213 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00214 *pb++ = (q15_t) __SSAT((coef), 16);
00215
00216
00217 tapCnt--;
00218 }
00219
00220
00221 x0 = *pState;
00222
00223
00224 pState = pState + 1u;
00225
00226
00227 blkCnt--;
00228 }
00229
00230
00231 S->energy = (q15_t) energy;
00232 S->x0 = x0;
00233
00234
00235
00236
00237
00238
00239 pStateCurnt = S->pState;
00240
00241
00242 tapCnt = (numTaps - 1u) >> 2;
00243
00244 while(tapCnt > 0u)
00245 {
00246
00247 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00248 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00249
00250 tapCnt--;
00251
00252 }
00253
00254
00255 tapCnt = (numTaps - 1u) % 0x4u;
00256
00257
00258 while(tapCnt > 0u)
00259 {
00260 *pStateCurnt++ = *pState++;
00261
00262
00263 tapCnt--;
00264 }
00265
00266 #else
00267
00268
00269
00270 while(blkCnt > 0u)
00271 {
00272
00273 *pStateCurnt++ = *pSrc;
00274
00275
00276 px = pState;
00277
00278
00279 pb = pCoeffs;
00280
00281
00282 in = *pSrc++;
00283
00284
00285 energy -= (((q31_t) x0 * (x0)) >> 15);
00286 energy += (((q31_t) in * (in)) >> 15);
00287
00288
00289 acc = 0;
00290
00291
00292 tapCnt = numTaps;
00293
00294 while(tapCnt > 0u)
00295 {
00296
00297 acc += (((q31_t) * px++ * (*pb++)));
00298
00299
00300 tapCnt--;
00301 }
00302
00303
00304 acc = __SSAT((acc >> (16u - shift)), 16u);
00305
00306
00307 *pOut++ = (q15_t) acc;
00308
00309
00310 d = *pRef++;
00311 e = d - (q15_t) acc;
00312 *pErr++ = e;
00313
00314
00315 postShift = arm_recip_q15((q15_t) energy + DELTA_Q15,
00316 &oneByEnergy, S->recipTable);
00317
00318
00319 errorXmu = (q15_t) (((q31_t) e * mu) >> 15);
00320
00321
00322 acc = (((q31_t) errorXmu * oneByEnergy) >> (15 - postShift));
00323
00324
00325 w = (q15_t) __SSAT((q31_t) acc, 16);
00326
00327
00328 px = pState;
00329
00330
00331 pb = (pCoeffs);
00332
00333
00334 tapCnt = numTaps;
00335
00336 while(tapCnt > 0u)
00337 {
00338
00339 coef = *pb + (((q31_t) w * (*px++)) >> 15);
00340 *pb++ = (q15_t) __SSAT((coef), 16);
00341
00342
00343 tapCnt--;
00344 }
00345
00346
00347 x0 = *pState;
00348
00349
00350 pState = pState + 1u;
00351
00352
00353 blkCnt--;
00354 }
00355
00356
00357 S->energy = (q15_t) energy;
00358 S->x0 = x0;
00359
00360
00361
00362
00363
00364
00365 pStateCurnt = S->pState;
00366
00367
00368 tapCnt = (numTaps - 1u);
00369
00370
00371 while(tapCnt > 0u)
00372 {
00373 *pStateCurnt++ = *pState++;
00374
00375
00376 tapCnt--;
00377 }
00378
00379 #endif
00380
00381 }
00382
00383