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