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
00066 void arm_lms_norm_q31(
00067 arm_lms_norm_instance_q31 * S,
00068 q31_t * pSrc,
00069 q31_t * pRef,
00070 q31_t * pOut,
00071 q31_t * pErr,
00072 uint32_t blockSize)
00073 {
00074 q31_t *pState = S->pState;
00075 q31_t *pCoeffs = S->pCoeffs;
00076 q31_t *pStateCurnt;
00077 q31_t *px, *pb;
00078 q31_t mu = S->mu;
00079 uint32_t numTaps = S->numTaps;
00080 uint32_t tapCnt, blkCnt;
00081 q63_t energy;
00082 q63_t acc;
00083 q31_t e = 0, d = 0;
00084 q31_t w = 0, in;
00085 q31_t x0;
00086 uint32_t shift = 32u - ((uint32_t) S->postShift + 1u);
00087 q31_t errorXmu, oneByEnergy;
00088 q31_t postShift;
00089 q31_t coef;
00090
00091 energy = S->energy;
00092 x0 = S->x0;
00093
00094
00095
00096 pStateCurnt = &(S->pState[(numTaps - 1u)]);
00097
00098 blkCnt = blockSize;
00099
00100 while(blkCnt > 0u)
00101 {
00102
00103
00104 *pStateCurnt++ = *pSrc;
00105
00106
00107 px = pState;
00108
00109
00110 pb = (pCoeffs);
00111
00112
00113 in = *pSrc++;
00114
00115
00116 energy = (q31_t) ((((q63_t) energy << 32) -
00117 (((q63_t) x0 * x0) << 1)) >> 32);
00118 energy = (q31_t) (((((q63_t) in * in) << 1) + (energy << 32)) >> 32);
00119
00120
00121 acc = 0;
00122
00123
00124 tapCnt = numTaps >> 2;
00125
00126 while(tapCnt > 0u)
00127 {
00128
00129 acc += ((q63_t) (*px++)) * (*pb++);
00130 acc += ((q63_t) (*px++)) * (*pb++);
00131 acc += ((q63_t) (*px++)) * (*pb++);
00132 acc += ((q63_t) (*px++)) * (*pb++);
00133
00134
00135 tapCnt--;
00136 }
00137
00138
00139 tapCnt = numTaps % 0x4u;
00140
00141 while(tapCnt > 0u)
00142 {
00143
00144 acc += ((q63_t) (*px++)) * (*pb++);
00145
00146
00147 tapCnt--;
00148 }
00149
00150
00151 acc = (q31_t) (acc >> shift);
00152
00153
00154 *pOut++ = (q31_t) acc;
00155
00156
00157 d = *pRef++;
00158 e = d - (q31_t) acc;
00159 *pErr++ = e;
00160
00161
00162 postShift = arm_recip_q31(energy + DELTA_Q31,
00163 &oneByEnergy, &S->recipTable[0]);
00164
00165
00166 errorXmu = (q31_t) (((q63_t) e * mu) >> 31);
00167
00168
00169 w = clip_q63_to_q31(((q63_t) errorXmu * oneByEnergy) >> (31 - postShift));
00170
00171
00172 px = pState;
00173
00174
00175 pb = (pCoeffs);
00176
00177
00178 tapCnt = numTaps >> 2;
00179
00180
00181 while(tapCnt > 0u)
00182 {
00183
00184
00185
00186 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
00187
00188 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u));
00189
00190 pb++;
00191
00192 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
00193 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u));
00194 pb++;
00195
00196 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
00197 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u));
00198 pb++;
00199
00200 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
00201 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u));
00202 pb++;
00203
00204
00205 tapCnt--;
00206 }
00207
00208
00209 tapCnt = numTaps % 0x4u;
00210
00211 while(tapCnt > 0u)
00212 {
00213
00214 coef = (q31_t) (((q63_t) w * (*px++)) >> (32));
00215 *pb = clip_q63_to_q31((q63_t) *pb + (coef << 1u));
00216 pb++;
00217
00218
00219 tapCnt--;
00220 }
00221
00222
00223 x0 = *pState;
00224
00225
00226 pState = pState + 1;
00227
00228
00229 blkCnt--;
00230 }
00231
00232
00233 S->energy = (q31_t) energy;
00234 S->x0 = x0;
00235
00236
00237
00238
00239
00240
00241 pStateCurnt = S->pState;
00242
00243
00244 tapCnt = (numTaps - 1u) >> 2u;
00245
00246
00247 while(tapCnt > 0u)
00248 {
00249 *pStateCurnt++ = *pState++;
00250 *pStateCurnt++ = *pState++;
00251 *pStateCurnt++ = *pState++;
00252 *pStateCurnt++ = *pState++;
00253
00254
00255 tapCnt--;
00256 }
00257
00258
00259 tapCnt = (numTaps - 1u) % 0x4u;
00260
00261
00262 while(tapCnt > 0u)
00263 {
00264 *pStateCurnt++ = *pState++;
00265
00266
00267 tapCnt--;
00268 }
00269
00270 }
00271