CMSIS DSP library

Dependents:   KL25Z_FFT_Demo Hat_Board_v5_1 KL25Z_FFT_Demo_tony KL25Z_FFT_Demo_tony ... more

Fork of mbed-dsp by mbed official

Committer:
emilmont
Date:
Wed Nov 28 12:30:09 2012 +0000
Revision:
1:fdd22bb7aa52
Child:
2:da51fb522205
DSP library code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
emilmont 1:fdd22bb7aa52 2 * Copyright (C) 2010 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
emilmont 1:fdd22bb7aa52 4 * $Date: 15. February 2012
emilmont 1:fdd22bb7aa52 5 * $Revision: V1.1.0
emilmont 1:fdd22bb7aa52 6 *
emilmont 1:fdd22bb7aa52 7 * Project: CMSIS DSP Library
emilmont 1:fdd22bb7aa52 8 * Title: arm_biquad_cascade_df1_fast_q31.c
emilmont 1:fdd22bb7aa52 9 *
emilmont 1:fdd22bb7aa52 10 * Description: Processing function for the
emilmont 1:fdd22bb7aa52 11 * Q31 Fast Biquad cascade DirectFormI(DF1) filter.
emilmont 1:fdd22bb7aa52 12 *
emilmont 1:fdd22bb7aa52 13 * Target Processor: Cortex-M4/Cortex-M3
emilmont 1:fdd22bb7aa52 14 *
emilmont 1:fdd22bb7aa52 15 * Version 1.1.0 2012/02/15
emilmont 1:fdd22bb7aa52 16 * Updated with more optimizations, bug fixes and minor API changes.
emilmont 1:fdd22bb7aa52 17 *
emilmont 1:fdd22bb7aa52 18 * Version 1.0.10 2011/7/15
emilmont 1:fdd22bb7aa52 19 * Big Endian support added and Merged M0 and M3/M4 Source code.
emilmont 1:fdd22bb7aa52 20 *
emilmont 1:fdd22bb7aa52 21 * Version 1.0.3 2010/11/29
emilmont 1:fdd22bb7aa52 22 * Re-organized the CMSIS folders and updated documentation.
emilmont 1:fdd22bb7aa52 23 *
emilmont 1:fdd22bb7aa52 24 * Version 1.0.2 2010/11/11
emilmont 1:fdd22bb7aa52 25 * Documentation updated.
emilmont 1:fdd22bb7aa52 26 *
emilmont 1:fdd22bb7aa52 27 * Version 1.0.1 2010/10/05
emilmont 1:fdd22bb7aa52 28 * Production release and review comments incorporated.
emilmont 1:fdd22bb7aa52 29 *
emilmont 1:fdd22bb7aa52 30 * Version 1.0.0 2010/09/20
emilmont 1:fdd22bb7aa52 31 * Production release and review comments incorporated.
emilmont 1:fdd22bb7aa52 32 *
emilmont 1:fdd22bb7aa52 33 * Version 0.0.9 2010/08/27
emilmont 1:fdd22bb7aa52 34 * Initial version
emilmont 1:fdd22bb7aa52 35 *
emilmont 1:fdd22bb7aa52 36 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 37
emilmont 1:fdd22bb7aa52 38 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 39
emilmont 1:fdd22bb7aa52 40 /**
emilmont 1:fdd22bb7aa52 41 * @ingroup groupFilters
emilmont 1:fdd22bb7aa52 42 */
emilmont 1:fdd22bb7aa52 43
emilmont 1:fdd22bb7aa52 44 /**
emilmont 1:fdd22bb7aa52 45 * @addtogroup BiquadCascadeDF1
emilmont 1:fdd22bb7aa52 46 * @{
emilmont 1:fdd22bb7aa52 47 */
emilmont 1:fdd22bb7aa52 48
emilmont 1:fdd22bb7aa52 49 /**
emilmont 1:fdd22bb7aa52 50 * @details
emilmont 1:fdd22bb7aa52 51 *
emilmont 1:fdd22bb7aa52 52 * @param[in] *S points to an instance of the Q31 Biquad cascade structure.
emilmont 1:fdd22bb7aa52 53 * @param[in] *pSrc points to the block of input data.
emilmont 1:fdd22bb7aa52 54 * @param[out] *pDst points to the block of output data.
emilmont 1:fdd22bb7aa52 55 * @param[in] blockSize number of samples to process per call.
emilmont 1:fdd22bb7aa52 56 * @return none.
emilmont 1:fdd22bb7aa52 57 *
emilmont 1:fdd22bb7aa52 58 * <b>Scaling and Overflow Behavior:</b>
emilmont 1:fdd22bb7aa52 59 * \par
emilmont 1:fdd22bb7aa52 60 * This function is optimized for speed at the expense of fixed-point precision and overflow protection.
emilmont 1:fdd22bb7aa52 61 * The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.
emilmont 1:fdd22bb7aa52 62 * These intermediate results are added to a 2.30 accumulator.
emilmont 1:fdd22bb7aa52 63 * Finally, the accumulator is saturated and converted to a 1.31 result.
emilmont 1:fdd22bb7aa52 64 * The fast version has the same overflow behavior as the standard version and provides less precision since it discards the low 32 bits of each multiplication result.
emilmont 1:fdd22bb7aa52 65 * In order to avoid overflows completely the input signal must be scaled down by two bits and lie in the range [-0.25 +0.25). Use the intialization function
emilmont 1:fdd22bb7aa52 66 * arm_biquad_cascade_df1_init_q31() to initialize filter structure.
emilmont 1:fdd22bb7aa52 67 *
emilmont 1:fdd22bb7aa52 68 * \par
emilmont 1:fdd22bb7aa52 69 * Refer to the function <code>arm_biquad_cascade_df1_q31()</code> for a slower implementation of this function which uses 64-bit accumulation to provide higher precision. Both the slow and the fast versions use the same instance structure.
emilmont 1:fdd22bb7aa52 70 * Use the function <code>arm_biquad_cascade_df1_init_q31()</code> to initialize the filter structure.
emilmont 1:fdd22bb7aa52 71 */
emilmont 1:fdd22bb7aa52 72
emilmont 1:fdd22bb7aa52 73 void arm_biquad_cascade_df1_fast_q31(
emilmont 1:fdd22bb7aa52 74 const arm_biquad_casd_df1_inst_q31 * S,
emilmont 1:fdd22bb7aa52 75 q31_t * pSrc,
emilmont 1:fdd22bb7aa52 76 q31_t * pDst,
emilmont 1:fdd22bb7aa52 77 uint32_t blockSize)
emilmont 1:fdd22bb7aa52 78 {
emilmont 1:fdd22bb7aa52 79 q31_t acc; /* accumulator */
emilmont 1:fdd22bb7aa52 80 q31_t Xn1, Xn2, Yn1, Yn2; /* Filter state variables */
emilmont 1:fdd22bb7aa52 81 q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
emilmont 1:fdd22bb7aa52 82 q31_t *pIn = pSrc; /* input pointer initialization */
emilmont 1:fdd22bb7aa52 83 q31_t *pOut = pDst; /* output pointer initialization */
emilmont 1:fdd22bb7aa52 84 q31_t *pState = S->pState; /* pState pointer initialization */
emilmont 1:fdd22bb7aa52 85 q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
emilmont 1:fdd22bb7aa52 86 q31_t Xn; /* temporary input */
emilmont 1:fdd22bb7aa52 87 int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
emilmont 1:fdd22bb7aa52 88 uint32_t sample, stage = S->numStages; /* loop counters */
emilmont 1:fdd22bb7aa52 89
emilmont 1:fdd22bb7aa52 90
emilmont 1:fdd22bb7aa52 91 do
emilmont 1:fdd22bb7aa52 92 {
emilmont 1:fdd22bb7aa52 93 /* Reading the coefficients */
emilmont 1:fdd22bb7aa52 94 b0 = *pCoeffs++;
emilmont 1:fdd22bb7aa52 95 b1 = *pCoeffs++;
emilmont 1:fdd22bb7aa52 96 b2 = *pCoeffs++;
emilmont 1:fdd22bb7aa52 97 a1 = *pCoeffs++;
emilmont 1:fdd22bb7aa52 98 a2 = *pCoeffs++;
emilmont 1:fdd22bb7aa52 99
emilmont 1:fdd22bb7aa52 100 /* Reading the state values */
emilmont 1:fdd22bb7aa52 101 Xn1 = pState[0];
emilmont 1:fdd22bb7aa52 102 Xn2 = pState[1];
emilmont 1:fdd22bb7aa52 103 Yn1 = pState[2];
emilmont 1:fdd22bb7aa52 104 Yn2 = pState[3];
emilmont 1:fdd22bb7aa52 105
emilmont 1:fdd22bb7aa52 106 /* Apply loop unrolling and compute 4 output values simultaneously. */
emilmont 1:fdd22bb7aa52 107 /* The variables acc ... acc3 hold output values that are being computed:
emilmont 1:fdd22bb7aa52 108 *
emilmont 1:fdd22bb7aa52 109 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
emilmont 1:fdd22bb7aa52 110 */
emilmont 1:fdd22bb7aa52 111
emilmont 1:fdd22bb7aa52 112 sample = blockSize >> 2u;
emilmont 1:fdd22bb7aa52 113
emilmont 1:fdd22bb7aa52 114 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
emilmont 1:fdd22bb7aa52 115 ** a second loop below computes the remaining 1 to 3 samples. */
emilmont 1:fdd22bb7aa52 116 while(sample > 0u)
emilmont 1:fdd22bb7aa52 117 {
emilmont 1:fdd22bb7aa52 118 /* Read the input */
emilmont 1:fdd22bb7aa52 119 Xn = *pIn;
emilmont 1:fdd22bb7aa52 120
emilmont 1:fdd22bb7aa52 121 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 122 /* acc = b0 * x[n] */
emilmont 1:fdd22bb7aa52 123 acc = (q31_t) (((q63_t) b1 * Xn1) >> 32);
emilmont 1:fdd22bb7aa52 124 /* acc += b1 * x[n-1] */
emilmont 1:fdd22bb7aa52 125 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b0 * (Xn))) >> 32);
emilmont 1:fdd22bb7aa52 126 /* acc += b[2] * x[n-2] */
emilmont 1:fdd22bb7aa52 127 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
emilmont 1:fdd22bb7aa52 128 /* acc += a1 * y[n-1] */
emilmont 1:fdd22bb7aa52 129 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
emilmont 1:fdd22bb7aa52 130 /* acc += a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 131 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
emilmont 1:fdd22bb7aa52 132
emilmont 1:fdd22bb7aa52 133 /* The result is converted to 1.31 , Yn2 variable is reused */
emilmont 1:fdd22bb7aa52 134 Yn2 = acc << shift;
emilmont 1:fdd22bb7aa52 135
emilmont 1:fdd22bb7aa52 136 /* Read the second input */
emilmont 1:fdd22bb7aa52 137 Xn2 = *(pIn + 1u);
emilmont 1:fdd22bb7aa52 138
emilmont 1:fdd22bb7aa52 139 /* Store the output in the destination buffer. */
emilmont 1:fdd22bb7aa52 140 *pOut = Yn2;
emilmont 1:fdd22bb7aa52 141
emilmont 1:fdd22bb7aa52 142 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 143 /* acc = b0 * x[n] */
emilmont 1:fdd22bb7aa52 144 acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);
emilmont 1:fdd22bb7aa52 145 /* acc += b1 * x[n-1] */
emilmont 1:fdd22bb7aa52 146 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);
emilmont 1:fdd22bb7aa52 147 /* acc += b[2] * x[n-2] */
emilmont 1:fdd22bb7aa52 148 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);
emilmont 1:fdd22bb7aa52 149 /* acc += a1 * y[n-1] */
emilmont 1:fdd22bb7aa52 150 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
emilmont 1:fdd22bb7aa52 151 /* acc += a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 152 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
emilmont 1:fdd22bb7aa52 153
emilmont 1:fdd22bb7aa52 154 /* The result is converted to 1.31, Yn1 variable is reused */
emilmont 1:fdd22bb7aa52 155 Yn1 = acc << shift;
emilmont 1:fdd22bb7aa52 156
emilmont 1:fdd22bb7aa52 157 /* Read the third input */
emilmont 1:fdd22bb7aa52 158 Xn1 = *(pIn + 2u);
emilmont 1:fdd22bb7aa52 159
emilmont 1:fdd22bb7aa52 160 /* Store the output in the destination buffer. */
emilmont 1:fdd22bb7aa52 161 *(pOut + 1u) = Yn1;
emilmont 1:fdd22bb7aa52 162
emilmont 1:fdd22bb7aa52 163 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 164 /* acc = b0 * x[n] */
emilmont 1:fdd22bb7aa52 165 acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);
emilmont 1:fdd22bb7aa52 166 /* acc += b1 * x[n-1] */
emilmont 1:fdd22bb7aa52 167 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);
emilmont 1:fdd22bb7aa52 168 /* acc += b[2] * x[n-2] */
emilmont 1:fdd22bb7aa52 169 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);
emilmont 1:fdd22bb7aa52 170 /* acc += a1 * y[n-1] */
emilmont 1:fdd22bb7aa52 171 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
emilmont 1:fdd22bb7aa52 172 /* acc += a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 173 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
emilmont 1:fdd22bb7aa52 174
emilmont 1:fdd22bb7aa52 175 /* The result is converted to 1.31, Yn2 variable is reused */
emilmont 1:fdd22bb7aa52 176 Yn2 = acc << shift;
emilmont 1:fdd22bb7aa52 177
emilmont 1:fdd22bb7aa52 178 /* Read the forth input */
emilmont 1:fdd22bb7aa52 179 Xn = *(pIn + 3u);
emilmont 1:fdd22bb7aa52 180
emilmont 1:fdd22bb7aa52 181 /* Store the output in the destination buffer. */
emilmont 1:fdd22bb7aa52 182 *(pOut + 2u) = Yn2;
emilmont 1:fdd22bb7aa52 183 pIn += 4u;
emilmont 1:fdd22bb7aa52 184
emilmont 1:fdd22bb7aa52 185 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 186 /* acc = b0 * x[n] */
emilmont 1:fdd22bb7aa52 187 acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
emilmont 1:fdd22bb7aa52 188 /* acc += b1 * x[n-1] */
emilmont 1:fdd22bb7aa52 189 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
emilmont 1:fdd22bb7aa52 190 /* acc += b[2] * x[n-2] */
emilmont 1:fdd22bb7aa52 191 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
emilmont 1:fdd22bb7aa52 192 /* acc += a1 * y[n-1] */
emilmont 1:fdd22bb7aa52 193 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);
emilmont 1:fdd22bb7aa52 194 /* acc += a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 195 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);
emilmont 1:fdd22bb7aa52 196
emilmont 1:fdd22bb7aa52 197 /* Every time after the output is computed state should be updated. */
emilmont 1:fdd22bb7aa52 198 /* The states should be updated as: */
emilmont 1:fdd22bb7aa52 199 /* Xn2 = Xn1 */
emilmont 1:fdd22bb7aa52 200 Xn2 = Xn1;
emilmont 1:fdd22bb7aa52 201
emilmont 1:fdd22bb7aa52 202 /* The result is converted to 1.31, Yn1 variable is reused */
emilmont 1:fdd22bb7aa52 203 Yn1 = acc << shift;
emilmont 1:fdd22bb7aa52 204
emilmont 1:fdd22bb7aa52 205 /* Xn1 = Xn */
emilmont 1:fdd22bb7aa52 206 Xn1 = Xn;
emilmont 1:fdd22bb7aa52 207
emilmont 1:fdd22bb7aa52 208 /* Store the output in the destination buffer. */
emilmont 1:fdd22bb7aa52 209 *(pOut + 3u) = Yn1;
emilmont 1:fdd22bb7aa52 210 pOut += 4u;
emilmont 1:fdd22bb7aa52 211
emilmont 1:fdd22bb7aa52 212 /* decrement the loop counter */
emilmont 1:fdd22bb7aa52 213 sample--;
emilmont 1:fdd22bb7aa52 214 }
emilmont 1:fdd22bb7aa52 215
emilmont 1:fdd22bb7aa52 216 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
emilmont 1:fdd22bb7aa52 217 ** No loop unrolling is used. */
emilmont 1:fdd22bb7aa52 218 sample = (blockSize & 0x3u);
emilmont 1:fdd22bb7aa52 219
emilmont 1:fdd22bb7aa52 220 while(sample > 0u)
emilmont 1:fdd22bb7aa52 221 {
emilmont 1:fdd22bb7aa52 222 /* Read the input */
emilmont 1:fdd22bb7aa52 223 Xn = *pIn++;
emilmont 1:fdd22bb7aa52 224
emilmont 1:fdd22bb7aa52 225 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 226 /* acc = b0 * x[n] */
emilmont 1:fdd22bb7aa52 227 acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);
emilmont 1:fdd22bb7aa52 228 /* acc += b1 * x[n-1] */
emilmont 1:fdd22bb7aa52 229 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);
emilmont 1:fdd22bb7aa52 230 /* acc += b[2] * x[n-2] */
emilmont 1:fdd22bb7aa52 231 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);
emilmont 1:fdd22bb7aa52 232 /* acc += a1 * y[n-1] */
emilmont 1:fdd22bb7aa52 233 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);
emilmont 1:fdd22bb7aa52 234 /* acc += a2 * y[n-2] */
emilmont 1:fdd22bb7aa52 235 acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);
emilmont 1:fdd22bb7aa52 236 /* The result is converted to 1.31 */
emilmont 1:fdd22bb7aa52 237 acc = acc << shift;
emilmont 1:fdd22bb7aa52 238
emilmont 1:fdd22bb7aa52 239 /* Every time after the output is computed state should be updated. */
emilmont 1:fdd22bb7aa52 240 /* The states should be updated as: */
emilmont 1:fdd22bb7aa52 241 /* Xn2 = Xn1 */
emilmont 1:fdd22bb7aa52 242 /* Xn1 = Xn */
emilmont 1:fdd22bb7aa52 243 /* Yn2 = Yn1 */
emilmont 1:fdd22bb7aa52 244 /* Yn1 = acc */
emilmont 1:fdd22bb7aa52 245 Xn2 = Xn1;
emilmont 1:fdd22bb7aa52 246 Xn1 = Xn;
emilmont 1:fdd22bb7aa52 247 Yn2 = Yn1;
emilmont 1:fdd22bb7aa52 248 Yn1 = acc;
emilmont 1:fdd22bb7aa52 249
emilmont 1:fdd22bb7aa52 250 /* Store the output in the destination buffer. */
emilmont 1:fdd22bb7aa52 251 *pOut++ = acc;
emilmont 1:fdd22bb7aa52 252
emilmont 1:fdd22bb7aa52 253 /* decrement the loop counter */
emilmont 1:fdd22bb7aa52 254 sample--;
emilmont 1:fdd22bb7aa52 255 }
emilmont 1:fdd22bb7aa52 256
emilmont 1:fdd22bb7aa52 257 /* The first stage goes from the input buffer to the output buffer. */
emilmont 1:fdd22bb7aa52 258 /* Subsequent stages occur in-place in the output buffer */
emilmont 1:fdd22bb7aa52 259 pIn = pDst;
emilmont 1:fdd22bb7aa52 260
emilmont 1:fdd22bb7aa52 261 /* Reset to destination pointer */
emilmont 1:fdd22bb7aa52 262 pOut = pDst;
emilmont 1:fdd22bb7aa52 263
emilmont 1:fdd22bb7aa52 264 /* Store the updated state variables back into the pState array */
emilmont 1:fdd22bb7aa52 265 *pState++ = Xn1;
emilmont 1:fdd22bb7aa52 266 *pState++ = Xn2;
emilmont 1:fdd22bb7aa52 267 *pState++ = Yn1;
emilmont 1:fdd22bb7aa52 268 *pState++ = Yn2;
emilmont 1:fdd22bb7aa52 269
emilmont 1:fdd22bb7aa52 270 } while(--stage);
emilmont 1:fdd22bb7aa52 271 }
emilmont 1:fdd22bb7aa52 272
emilmont 1:fdd22bb7aa52 273 /**
emilmont 1:fdd22bb7aa52 274 * @} end of BiquadCascadeDF1 group
emilmont 1:fdd22bb7aa52 275 */