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
00062 void arm_iir_lattice_q31(
00063 const arm_iir_lattice_instance_q31 * S,
00064 q31_t * pSrc,
00065 q31_t * pDst,
00066 uint32_t blockSize)
00067 {
00068 q31_t fcurr, fnext = 0, gcurr = 0, gnext;
00069 q63_t acc;
00070 uint32_t blkCnt, tapCnt;
00071 q31_t *px1, *px2, *pk, *pv;
00072 uint32_t numStages = S->numStages;
00073 q31_t *pState;
00074 q31_t *pStateCurnt;
00075
00076 blkCnt = blockSize;
00077
00078 pState = &S->pState[0];
00079
00080
00081 #ifndef ARM_MATH_CM0
00082
00083
00084
00085
00086 while(blkCnt > 0u)
00087 {
00088
00089
00090 fcurr = *pSrc++;
00091
00092
00093 px1 = pState;
00094
00095 px2 = pState;
00096
00097 acc = 0;
00098
00099 pv = &S->pvCoeffs[0];
00100
00101 pk = &S->pkCoeffs[0];
00102
00103
00104
00105 gcurr = *px1++;
00106
00107 fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00108
00109 gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
00110
00111 *px2++ = gnext;
00112
00113 acc += ((q63_t) gnext * *pv++);
00114
00115
00116 fcurr = fnext;
00117
00118
00119 tapCnt = (numStages - 1u) >> 2;
00120
00121 while(tapCnt > 0u)
00122 {
00123
00124
00125
00126 gcurr = *px1++;
00127
00128 fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00129
00130 gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
00131
00132
00133 acc += ((q63_t) gnext * *pv++);
00134
00135 *px2++ = gnext;
00136
00137
00138
00139 gcurr = *px1++;
00140
00141
00142 fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00143
00144 gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
00145
00146
00147 acc += ((q63_t) gnext * *pv++);
00148
00149 *px2++ = gnext;
00150
00151
00152
00153
00154 gcurr = *px1++;
00155
00156
00157 fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00158
00159 gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
00160
00161
00162 acc += ((q63_t) gnext * *pv++);
00163
00164 *px2++ = gnext;
00165
00166
00167
00168
00169 gcurr = *px1++;
00170
00171
00172 fcurr = __QSUB(fnext, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00173
00174 gnext = __QADD(gcurr, (q31_t) (((q63_t) fcurr * (*pk++)) >> 31));
00175
00176
00177 acc += ((q63_t) gnext * *pv++);
00178
00179 *px2++ = gnext;
00180
00181 tapCnt--;
00182
00183 }
00184
00185 fnext = fcurr;
00186
00187
00188 tapCnt = (numStages - 1u) % 0x4u;
00189
00190 while(tapCnt > 0u)
00191 {
00192 gcurr = *px1++;
00193
00194 fnext = __QSUB(fcurr, (q31_t) (((q63_t) gcurr * (*pk)) >> 31));
00195 gnext = __QADD(gcurr, (q31_t) (((q63_t) fnext * (*pk++)) >> 31));
00196
00197 acc += ((q63_t) gnext * *pv++);
00198 *px2++ = gnext;
00199 fcurr = fnext;
00200
00201 tapCnt--;
00202
00203 }
00204
00205
00206 acc += (q63_t) fnext *(
00207 *pv++);
00208
00209 *px2++ = fnext;
00210
00211
00212 *pDst++ = (q31_t) (acc >> 31u);
00213
00214
00215 pState = pState + 1u;
00216 blkCnt--;
00217
00218 }
00219
00220
00221
00222
00223
00224 pStateCurnt = &S->pState[0];
00225 pState = &S->pState[blockSize];
00226
00227 tapCnt = numStages >> 2u;
00228
00229
00230 while(tapCnt > 0u)
00231 {
00232 *pStateCurnt++ = *pState++;
00233 *pStateCurnt++ = *pState++;
00234 *pStateCurnt++ = *pState++;
00235 *pStateCurnt++ = *pState++;
00236
00237
00238 tapCnt--;
00239
00240 }
00241
00242
00243 tapCnt = (numStages) % 0x4u;
00244
00245
00246 while(tapCnt > 0u)
00247 {
00248 *pStateCurnt++ = *pState++;
00249
00250
00251 tapCnt--;
00252 };
00253
00254 #else
00255
00256
00257
00258 while(blkCnt > 0u)
00259 {
00260
00261
00262 fcurr = *pSrc++;
00263
00264
00265 px1 = pState;
00266
00267 px2 = pState;
00268
00269 acc = 0;
00270
00271 pv = &S->pvCoeffs[0];
00272
00273 pk = &S->pkCoeffs[0];
00274
00275 tapCnt = numStages;
00276
00277 while(tapCnt > 0u)
00278 {
00279 gcurr = *px1++;
00280
00281
00282 fnext =
00283 clip_q63_to_q31(((q63_t) fcurr -
00284 ((q31_t) (((q63_t) gcurr * (*pk)) >> 31))));
00285
00286 gnext =
00287 clip_q63_to_q31(((q63_t) gcurr +
00288 ((q31_t) (((q63_t) fnext * (*pk++)) >> 31))));
00289
00290
00291 acc += ((q63_t) gnext * *pv++);
00292
00293 *px2++ = gnext;
00294
00295 fcurr = fnext;
00296
00297 tapCnt--;
00298 }
00299
00300
00301 acc += (q63_t) fnext *(
00302 *pv++);
00303
00304 *px2++ = fnext;
00305
00306
00307 *pDst++ = (q31_t) (acc >> 31u);
00308
00309
00310 pState = pState + 1u;
00311 blkCnt--;
00312
00313 }
00314
00315
00316
00317
00318
00319 pStateCurnt = &S->pState[0];
00320 pState = &S->pState[blockSize];
00321
00322 tapCnt = numStages;
00323
00324
00325 while(tapCnt > 0u)
00326 {
00327 *pStateCurnt++ = *pState++;
00328
00329
00330 tapCnt--;
00331 }
00332
00333 #endif
00334
00335 }
00336
00337
00338
00339