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