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