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_q15(
00064 const arm_lms_instance_q15 * S,
00065 q15_t * pSrc,
00066 q15_t * pRef,
00067 q15_t * pOut,
00068 q15_t * pErr,
00069 uint32_t blockSize)
00070 {
00071 q15_t *pState = S->pState;
00072 uint32_t numTaps = S->numTaps;
00073 q15_t *pCoeffs = S->pCoeffs;
00074 q15_t *pStateCurnt;
00075 q15_t mu = S->mu;
00076 q15_t *px;
00077 q15_t *pb;
00078 uint32_t tapCnt, blkCnt;
00079 q63_t acc;
00080 q15_t e = 0;
00081 q15_t alpha;
00082 uint32_t shift = 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 >> 2u;
00108
00109 while(tapCnt > 0u)
00110 {
00111
00112
00113 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00114 acc = __SMLALD(*__SIMD32(px)++, (*__SIMD32(pb)++), acc);
00115
00116
00117 tapCnt--;
00118 }
00119
00120
00121 tapCnt = numTaps % 0x4u;
00122
00123 while(tapCnt > 0u)
00124 {
00125
00126 acc += (q63_t) (((q31_t) (*px++) * (*pb++)));
00127
00128
00129 tapCnt--;
00130 }
00131
00132
00133 acc = __SSAT((acc >> (16 - shift)), 16);
00134
00135
00136 *pOut++ = (q15_t) acc;
00137
00138
00139 e = *pRef++ - (q15_t) acc;
00140
00141 *pErr++ = (q15_t) e;
00142
00143
00144 alpha = (q15_t) (((q31_t) e * (mu)) >> 15);
00145
00146
00147
00148 px = pState++;
00149
00150
00151 pb = pCoeffs;
00152
00153
00154 tapCnt = numTaps >> 2u;
00155
00156
00157 while(tapCnt > 0u)
00158 {
00159 coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
00160 *pb++ = (q15_t) __SSAT((coef), 16);
00161 coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
00162 *pb++ = (q15_t) __SSAT((coef), 16);
00163 coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
00164 *pb++ = (q15_t) __SSAT((coef), 16);
00165 coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
00166 *pb++ = (q15_t) __SSAT((coef), 16);
00167
00168
00169 tapCnt--;
00170 }
00171
00172
00173 tapCnt = numTaps % 0x4u;
00174
00175 while(tapCnt > 0u)
00176 {
00177
00178 coef = (q31_t) *pb + (((q31_t) alpha * (*px++)) >> 15);
00179 *pb++ = (q15_t) __SSAT((coef), 16);
00180
00181
00182 tapCnt--;
00183 }
00184
00185
00186 blkCnt--;
00187
00188 }
00189
00190
00191
00192
00193
00194
00195 pStateCurnt = S->pState;
00196
00197
00198 tapCnt = (numTaps - 1u) >> 2;
00199
00200 while(tapCnt > 0u)
00201 {
00202
00203 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00204 *__SIMD32(pStateCurnt)++ = *__SIMD32(pState)++;
00205
00206 tapCnt--;
00207
00208 }
00209
00210
00211 tapCnt = (numTaps - 1u) % 0x4u;
00212
00213
00214 while(tapCnt > 0u)
00215 {
00216 *pStateCurnt++ = *pState++;
00217
00218
00219 tapCnt--;
00220 }
00221
00222 }
00223