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 #include "arm_math.h"
00033
00067 void arm_biquad_cascade_df1_fast_q31(
00068 const arm_biquad_casd_df1_inst_q31 * S,
00069 q31_t * pSrc,
00070 q31_t * pDst,
00071 uint32_t blockSize)
00072 {
00073 q31_t *pIn = pSrc;
00074 q31_t *pOut = pDst;
00075 q31_t *pState = S->pState;
00076 q31_t *pCoeffs = S->pCoeffs;
00077 q31_t acc;
00078 q31_t Xn1, Xn2, Yn1, Yn2;
00079 q31_t b0, b1, b2, a1, a2;
00080 q31_t Xn;
00081 int32_t shift = (int32_t) S->postShift + 1;
00082 uint32_t sample, stage = S->numStages;
00083
00084
00085 do
00086 {
00087
00088 b0 = *pCoeffs++;
00089 b1 = *pCoeffs++;
00090 b2 = *pCoeffs++;
00091 a1 = *pCoeffs++;
00092 a2 = *pCoeffs++;
00093
00094
00095 Xn1 = pState[0];
00096 Xn2 = pState[1];
00097 Yn1 = pState[2];
00098 Yn2 = pState[3];
00099
00100
00101
00102
00103
00104
00105
00106 sample = blockSize >> 2u;
00107
00108
00109
00110 while(sample > 0u)
00111 {
00112
00113 Xn = *pIn++;
00114
00115
00116
00117 acc = (q31_t) (((q63_t) b0 * Xn) >> 32);
00118
00119 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
00120
00121 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
00122
00123 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
00124
00125 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
00126
00127
00128 Yn2 = acc << shift;
00129
00130
00131 *pOut++ = Yn2;
00132
00133
00134 Xn2 = *pIn++;
00135
00136
00137
00138 acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);
00139
00140 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);
00141
00142 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);
00143
00144 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
00145
00146 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
00147
00148
00149 Yn1 = acc << shift;
00150
00151
00152 *pOut++ = Yn1;
00153
00154
00155 Xn1 = *pIn++;
00156
00157
00158
00159 acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);
00160
00161 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);
00162
00163 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);
00164
00165 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
00166
00167 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
00168
00169
00170 Yn2 = acc << shift;
00171
00172
00173 *pOut++ = Yn2;
00174
00175
00176 Xn = *pIn++;
00177
00178
00179
00180 acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
00181
00182 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
00183
00184 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
00185
00186 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
00187
00188 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
00189
00190
00191 Yn1 = acc << shift;
00192
00193
00194
00195
00196
00197
00198
00199 Xn2 = Xn1;
00200 Xn1 = Xn;
00201
00202
00203 *pOut++ = Yn1;
00204
00205
00206 sample--;
00207 }
00208
00209
00210
00211 sample = (blockSize & 0x3u);
00212
00213 while(sample > 0u)
00214 {
00215
00216 Xn = *pIn++;
00217
00218
00219
00220 acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
00221
00222 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
00223
00224 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
00225
00226 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
00227
00228 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
00229
00230 acc = acc << shift;
00231
00232
00233
00234
00235
00236
00237
00238 Xn2 = Xn1;
00239 Xn1 = Xn;
00240 Yn2 = Yn1;
00241 Yn1 = acc;
00242
00243
00244 *pOut++ = acc;
00245
00246
00247 sample--;
00248 }
00249
00250
00251
00252 pIn = pDst;
00253
00254
00255 pOut = pDst;
00256
00257
00258 *pState++ = Xn1;
00259 *pState++ = Xn2;
00260 *pState++ = Yn1;
00261 *pState++ = Yn2;
00262
00263 } while(--stage);
00264 }
00265