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