Eli Hughes / CMSIS_DSP_401

V4.0.1 of the ARM CMSIS DSP libraries. Note that arm_bitreversal2.s, arm_cfft_f32.c and arm_rfft_fast_f32.c had to be removed. arm_bitreversal2.s will not assemble with the online tools. So, the fast f32 FFT functions are not yet available. All the other FFT functions are available.

Dependents:   MPU9150_Example fir_f32 fir_f32 MPU9150_nucleo_noni2cdev ... more

FilteringFunctions/arm_biquad_cascade_df1_32x64_q31.c@0:3d9c67d97d6f, 2014-07-28 (annotated)

Committer:
emh203
Date:
Mon Jul 28 15:03:15 2014 +0000
Revision:
0:3d9c67d97d6f
1st working commit.   Had to remove arm_bitreversal2.s     arm_cfft_f32.c and arm_rfft_fast_f32.c.    The .s will not assemble.      For now I removed these functions so we could at least have a library for the other functions.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emh203 0:3d9c67d97d6f 1 /* ----------------------------------------------------------------------
emh203 0:3d9c67d97d6f 2 * Copyright (C) 2010-2014 ARM Limited. All rights reserved.
emh203 0:3d9c67d97d6f 3 *
emh203 0:3d9c67d97d6f 4 * $Date: 12. March 2014
emh203 0:3d9c67d97d6f 5 * $Revision: V1.4.3
emh203 0:3d9c67d97d6f 6 *
emh203 0:3d9c67d97d6f 7 * Project: CMSIS DSP Library
emh203 0:3d9c67d97d6f 8 * Title: arm_biquad_cascade_df1_32x64_q31.c
emh203 0:3d9c67d97d6f 9 *
emh203 0:3d9c67d97d6f 10 * Description: High precision Q31 Biquad cascade filter processing function
emh203 0:3d9c67d97d6f 11 *
emh203 0:3d9c67d97d6f 12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
emh203 0:3d9c67d97d6f 13 *
emh203 0:3d9c67d97d6f 14 * Redistribution and use in source and binary forms, with or without
emh203 0:3d9c67d97d6f 15 * modification, are permitted provided that the following conditions
emh203 0:3d9c67d97d6f 16 * are met:
emh203 0:3d9c67d97d6f 17 * - Redistributions of source code must retain the above copyright
emh203 0:3d9c67d97d6f 18 * notice, this list of conditions and the following disclaimer.
emh203 0:3d9c67d97d6f 19 * - Redistributions in binary form must reproduce the above copyright
emh203 0:3d9c67d97d6f 20 * notice, this list of conditions and the following disclaimer in
emh203 0:3d9c67d97d6f 21 * the documentation and/or other materials provided with the
emh203 0:3d9c67d97d6f 22 * distribution.
emh203 0:3d9c67d97d6f 23 * - Neither the name of ARM LIMITED nor the names of its contributors
emh203 0:3d9c67d97d6f 24 * may be used to endorse or promote products derived from this
emh203 0:3d9c67d97d6f 25 * software without specific prior written permission.
emh203 0:3d9c67d97d6f 26 *
emh203 0:3d9c67d97d6f 27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
emh203 0:3d9c67d97d6f 28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
emh203 0:3d9c67d97d6f 29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
emh203 0:3d9c67d97d6f 30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
emh203 0:3d9c67d97d6f 31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
emh203 0:3d9c67d97d6f 32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
emh203 0:3d9c67d97d6f 33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
emh203 0:3d9c67d97d6f 34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
emh203 0:3d9c67d97d6f 35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
emh203 0:3d9c67d97d6f 36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
emh203 0:3d9c67d97d6f 37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
emh203 0:3d9c67d97d6f 38 * POSSIBILITY OF SUCH DAMAGE.
emh203 0:3d9c67d97d6f 39 * -------------------------------------------------------------------- */
emh203 0:3d9c67d97d6f 40
emh203 0:3d9c67d97d6f 41 #include "arm_math.h"
emh203 0:3d9c67d97d6f 42
emh203 0:3d9c67d97d6f 43 /**
emh203 0:3d9c67d97d6f 44 * @ingroup groupFilters
emh203 0:3d9c67d97d6f 45 */
emh203 0:3d9c67d97d6f 46
emh203 0:3d9c67d97d6f 47 /**
emh203 0:3d9c67d97d6f 48 * @defgroup BiquadCascadeDF1_32x64 High Precision Q31 Biquad Cascade Filter
emh203 0:3d9c67d97d6f 49 *
emh203 0:3d9c67d97d6f 50 * This function implements a high precision Biquad cascade filter which operates on
emh203 0:3d9c67d97d6f 51 * Q31 data values. The filter coefficients are in 1.31 format and the state variables
emh203 0:3d9c67d97d6f 52 * are in 1.63 format. The double precision state variables reduce quantization noise
emh203 0:3d9c67d97d6f 53 * in the filter and provide a cleaner output.
emh203 0:3d9c67d97d6f 54 * These filters are particularly useful when implementing filters in which the
emh203 0:3d9c67d97d6f 55 * singularities are close to the unit circle. This is common for low pass or high
emh203 0:3d9c67d97d6f 56 * pass filters with very low cutoff frequencies.
emh203 0:3d9c67d97d6f 57 *
emh203 0:3d9c67d97d6f 58 * The function operates on blocks of input and output data
emh203 0:3d9c67d97d6f 59 * and each call to the function processes <code>blockSize</code> samples through
emh203 0:3d9c67d97d6f 60 * the filter. <code>pSrc</code> and <code>pDst</code> points to input and output arrays
emh203 0:3d9c67d97d6f 61 * containing <code>blockSize</code> Q31 values.
emh203 0:3d9c67d97d6f 62 *
emh203 0:3d9c67d97d6f 63 * \par Algorithm
emh203 0:3d9c67d97d6f 64 * Each Biquad stage implements a second order filter using the difference equation:
emh203 0:3d9c67d97d6f 65 * <pre>
emh203 0:3d9c67d97d6f 66 * y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
emh203 0:3d9c67d97d6f 67 * </pre>
emh203 0:3d9c67d97d6f 68 * A Direct Form I algorithm is used with 5 coefficients and 4 state variables per stage.
emh203 0:3d9c67d97d6f 69 * \image html Biquad.gif "Single Biquad filter stage"
emh203 0:3d9c67d97d6f 70 * Coefficients <code>b0, b1, and b2 </code> multiply the input signal <code>x[n]</code> and are referred to as the feedforward coefficients.
emh203 0:3d9c67d97d6f 71 * Coefficients <code>a1</code> and <code>a2</code> multiply the output signal <code>y[n]</code> and are referred to as the feedback coefficients.
emh203 0:3d9c67d97d6f 72 * Pay careful attention to the sign of the feedback coefficients.
emh203 0:3d9c67d97d6f 73 * Some design tools use the difference equation
emh203 0:3d9c67d97d6f 74 * <pre>
emh203 0:3d9c67d97d6f 75 * y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] - a1 * y[n-1] - a2 * y[n-2]
emh203 0:3d9c67d97d6f 76 * </pre>
emh203 0:3d9c67d97d6f 77 * In this case the feedback coefficients <code>a1</code> and <code>a2</code> must be negated when used with the CMSIS DSP Library.
emh203 0:3d9c67d97d6f 78 *
emh203 0:3d9c67d97d6f 79 * \par
emh203 0:3d9c67d97d6f 80 * Higher order filters are realized as a cascade of second order sections.
emh203 0:3d9c67d97d6f 81 * <code>numStages</code> refers to the number of second order stages used.
emh203 0:3d9c67d97d6f 82 * For example, an 8th order filter would be realized with <code>numStages=4</code> second order stages.
emh203 0:3d9c67d97d6f 83 * \image html BiquadCascade.gif "8th order filter using a cascade of Biquad stages"
emh203 0:3d9c67d97d6f 84 * A 9th order filter would be realized with <code>numStages=5</code> second order stages with the coefficients for one of the stages configured as a first order filter (<code>b2=0</code> and <code>a2=0</code>).
emh203 0:3d9c67d97d6f 85 *
emh203 0:3d9c67d97d6f 86 * \par
emh203 0:3d9c67d97d6f 87 * The <code>pState</code> points to state variables array .
emh203 0:3d9c67d97d6f 88 * Each Biquad stage has 4 state variables <code>x[n-1], x[n-2], y[n-1],</code> and <code>y[n-2]</code> and each state variable in 1.63 format to improve precision.
emh203 0:3d9c67d97d6f 89 * The state variables are arranged in the array as:
emh203 0:3d9c67d97d6f 90 * <pre>
emh203 0:3d9c67d97d6f 91 * {x[n-1], x[n-2], y[n-1], y[n-2]}
emh203 0:3d9c67d97d6f 92 * </pre>
emh203 0:3d9c67d97d6f 93 *
emh203 0:3d9c67d97d6f 94 * \par
emh203 0:3d9c67d97d6f 95 * The 4 state variables for stage 1 are first, then the 4 state variables for stage 2, and so on.
emh203 0:3d9c67d97d6f 96 * The state array has a total length of <code>4*numStages</code> values of data in 1.63 format.
emh203 0:3d9c67d97d6f 97 * The state variables are updated after each block of data is processed; the coefficients are untouched.
emh203 0:3d9c67d97d6f 98 *
emh203 0:3d9c67d97d6f 99 * \par Instance Structure
emh203 0:3d9c67d97d6f 100 * The coefficients and state variables for a filter are stored together in an instance data structure.
emh203 0:3d9c67d97d6f 101 * A separate instance structure must be defined for each filter.
emh203 0:3d9c67d97d6f 102 * Coefficient arrays may be shared among several instances while state variable arrays cannot be shared.
emh203 0:3d9c67d97d6f 103 *
emh203 0:3d9c67d97d6f 104 * \par Init Function
emh203 0:3d9c67d97d6f 105 * There is also an associated initialization function which performs the following operations:
emh203 0:3d9c67d97d6f 106 * - Sets the values of the internal structure fields.
emh203 0:3d9c67d97d6f 107 * - Zeros out the values in the state buffer.
emh203 0:3d9c67d97d6f 108 * To do this manually without calling the init function, assign the follow subfields of the instance structure:
emh203 0:3d9c67d97d6f 109 * numStages, pCoeffs, postShift, pState. Also set all of the values in pState to zero.
emh203 0:3d9c67d97d6f 110 *
emh203 0:3d9c67d97d6f 111 * \par
emh203 0:3d9c67d97d6f 112 * Use of the initialization function is optional.
emh203 0:3d9c67d97d6f 113 * However, if the initialization function is used, then the instance structure cannot be placed into a const data section.
emh203 0:3d9c67d97d6f 114 * To place an instance structure into a const data section, the instance structure must be manually initialized.
emh203 0:3d9c67d97d6f 115 * Set the values in the state buffer to zeros before static initialization.
emh203 0:3d9c67d97d6f 116 * For example, to statically initialize the filter instance structure use
emh203 0:3d9c67d97d6f 117 * <pre>
emh203 0:3d9c67d97d6f 118 * arm_biquad_cas_df1_32x64_ins_q31 S1 = {numStages, pState, pCoeffs, postShift};
emh203 0:3d9c67d97d6f 119 * </pre>
emh203 0:3d9c67d97d6f 120 * where <code>numStages</code> is the number of Biquad stages in the filter; <code>pState</code> is the address of the state buffer;
emh203 0:3d9c67d97d6f 121 * <code>pCoeffs</code> is the address of the coefficient buffer; <code>postShift</code> shift to be applied which is described in detail below.
emh203 0:3d9c67d97d6f 122 * \par Fixed-Point Behavior
emh203 0:3d9c67d97d6f 123 * Care must be taken while using Biquad Cascade 32x64 filter function.
emh203 0:3d9c67d97d6f 124 * Following issues must be considered:
emh203 0:3d9c67d97d6f 125 * - Scaling of coefficients
emh203 0:3d9c67d97d6f 126 * - Filter gain
emh203 0:3d9c67d97d6f 127 * - Overflow and saturation
emh203 0:3d9c67d97d6f 128 *
emh203 0:3d9c67d97d6f 129 * \par
emh203 0:3d9c67d97d6f 130 * Filter coefficients are represented as fractional values and
emh203 0:3d9c67d97d6f 131 * restricted to lie in the range <code>[-1 +1)</code>.
emh203 0:3d9c67d97d6f 132 * The processing function has an additional scaling parameter <code>postShift</code>
emh203 0:3d9c67d97d6f 133 * which allows the filter coefficients to exceed the range <code>[+1 -1)</code>.
emh203 0:3d9c67d97d6f 134 * At the output of the filter's accumulator is a shift register which shifts the result by <code>postShift</code> bits.
emh203 0:3d9c67d97d6f 135 * \image html BiquadPostshift.gif "Fixed-point Biquad with shift by postShift bits after accumulator"
emh203 0:3d9c67d97d6f 136 * This essentially scales the filter coefficients by <code>2^postShift</code>.
emh203 0:3d9c67d97d6f 137 * For example, to realize the coefficients
emh203 0:3d9c67d97d6f 138 * <pre>
emh203 0:3d9c67d97d6f 139 * {1.5, -0.8, 1.2, 1.6, -0.9}
emh203 0:3d9c67d97d6f 140 * </pre>
emh203 0:3d9c67d97d6f 141 * set the Coefficient array to:
emh203 0:3d9c67d97d6f 142 * <pre>
emh203 0:3d9c67d97d6f 143 * {0.75, -0.4, 0.6, 0.8, -0.45}
emh203 0:3d9c67d97d6f 144 * </pre>
emh203 0:3d9c67d97d6f 145 * and set <code>postShift=1</code>
emh203 0:3d9c67d97d6f 146 *
emh203 0:3d9c67d97d6f 147 * \par
emh203 0:3d9c67d97d6f 148 * The second thing to keep in mind is the gain through the filter.
emh203 0:3d9c67d97d6f 149 * The frequency response of a Biquad filter is a function of its coefficients.
emh203 0:3d9c67d97d6f 150 * It is possible for the gain through the filter to exceed 1.0 meaning that the filter increases the amplitude of certain frequencies.
emh203 0:3d9c67d97d6f 151 * This means that an input signal with amplitude < 1.0 may result in an output > 1.0 and these are saturated or overflowed based on the implementation of the filter.
emh203 0:3d9c67d97d6f 152 * To avoid this behavior the filter needs to be scaled down such that its peak gain < 1.0 or the input signal must be scaled down so that the combination of input and filter are never overflowed.
emh203 0:3d9c67d97d6f 153 *
emh203 0:3d9c67d97d6f 154 * \par
emh203 0:3d9c67d97d6f 155 * The third item to consider is the overflow and saturation behavior of the fixed-point Q31 version.
emh203 0:3d9c67d97d6f 156 * This is described in the function specific documentation below.
emh203 0:3d9c67d97d6f 157 */
emh203 0:3d9c67d97d6f 158
emh203 0:3d9c67d97d6f 159 /**
emh203 0:3d9c67d97d6f 160 * @addtogroup BiquadCascadeDF1_32x64
emh203 0:3d9c67d97d6f 161 * @{
emh203 0:3d9c67d97d6f 162 */
emh203 0:3d9c67d97d6f 163
emh203 0:3d9c67d97d6f 164 /**
emh203 0:3d9c67d97d6f 165 * @details
emh203 0:3d9c67d97d6f 166
emh203 0:3d9c67d97d6f 167 * @param[in] *S points to an instance of the high precision Q31 Biquad cascade filter.
emh203 0:3d9c67d97d6f 168 * @param[in] *pSrc points to the block of input data.
emh203 0:3d9c67d97d6f 169 * @param[out] *pDst points to the block of output data.
emh203 0:3d9c67d97d6f 170 * @param[in] blockSize number of samples to process.
emh203 0:3d9c67d97d6f 171 * @return none.
emh203 0:3d9c67d97d6f 172 *
emh203 0:3d9c67d97d6f 173 * \par
emh203 0:3d9c67d97d6f 174 * The function is implemented using an internal 64-bit accumulator.
emh203 0:3d9c67d97d6f 175 * The accumulator has a 2.62 format and maintains full precision of the intermediate multiplication results but provides only a single guard bit.
emh203 0:3d9c67d97d6f 176 * Thus, if the accumulator result overflows it wraps around rather than clip.
emh203 0:3d9c67d97d6f 177 * In order to avoid overflows completely the input signal must be scaled down by 2 bits and lie in the range [-0.25 +0.25).
emh203 0:3d9c67d97d6f 178 * After all 5 multiply-accumulates are performed, the 2.62 accumulator is shifted by <code>postShift</code> bits and the result truncated to
emh203 0:3d9c67d97d6f 179 * 1.31 format by discarding the low 32 bits.
emh203 0:3d9c67d97d6f 180 *
emh203 0:3d9c67d97d6f 181 * \par
emh203 0:3d9c67d97d6f 182 * Two related functions are provided in the CMSIS DSP library.
emh203 0:3d9c67d97d6f 183 * <code>arm_biquad_cascade_df1_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q63 accumulator.
emh203 0:3d9c67d97d6f 184 * <code>arm_biquad_cascade_df1_fast_q31()</code> implements a Biquad cascade with 32-bit coefficients and state variables with a Q31 accumulator.
emh203 0:3d9c67d97d6f 185 */
emh203 0:3d9c67d97d6f 186
emh203 0:3d9c67d97d6f 187 void arm_biquad_cas_df1_32x64_q31(
emh203 0:3d9c67d97d6f 188 const arm_biquad_cas_df1_32x64_ins_q31 * S,
emh203 0:3d9c67d97d6f 189 q31_t * pSrc,
emh203 0:3d9c67d97d6f 190 q31_t * pDst,
emh203 0:3d9c67d97d6f 191 uint32_t blockSize)
emh203 0:3d9c67d97d6f 192 {
emh203 0:3d9c67d97d6f 193 q31_t *pIn = pSrc; /* input pointer initialization */
emh203 0:3d9c67d97d6f 194 q31_t *pOut = pDst; /* output pointer initialization */
emh203 0:3d9c67d97d6f 195 q63_t *pState = S->pState; /* state pointer initialization */
emh203 0:3d9c67d97d6f 196 q31_t *pCoeffs = S->pCoeffs; /* coeff pointer initialization */
emh203 0:3d9c67d97d6f 197 q63_t acc; /* accumulator */
emh203 0:3d9c67d97d6f 198 q31_t Xn1, Xn2; /* Input Filter state variables */
emh203 0:3d9c67d97d6f 199 q63_t Yn1, Yn2; /* Output Filter state variables */
emh203 0:3d9c67d97d6f 200 q31_t b0, b1, b2, a1, a2; /* Filter coefficients */
emh203 0:3d9c67d97d6f 201 q31_t Xn; /* temporary input */
emh203 0:3d9c67d97d6f 202 int32_t shift = (int32_t) S->postShift + 1; /* Shift to be applied to the output */
emh203 0:3d9c67d97d6f 203 uint32_t sample, stage = S->numStages; /* loop counters */
emh203 0:3d9c67d97d6f 204 q31_t acc_l, acc_h; /* temporary output */
emh203 0:3d9c67d97d6f 205 uint32_t uShift = ((uint32_t) S->postShift + 1u);
emh203 0:3d9c67d97d6f 206 uint32_t lShift = 32u - uShift; /* Shift to be applied to the output */
emh203 0:3d9c67d97d6f 207
emh203 0:3d9c67d97d6f 208
emh203 0:3d9c67d97d6f 209 #ifndef ARM_MATH_CM0_FAMILY
emh203 0:3d9c67d97d6f 210
emh203 0:3d9c67d97d6f 211 /* Run the below code for Cortex-M4 and Cortex-M3 */
emh203 0:3d9c67d97d6f 212
emh203 0:3d9c67d97d6f 213 do
emh203 0:3d9c67d97d6f 214 {
emh203 0:3d9c67d97d6f 215 /* Reading the coefficients */
emh203 0:3d9c67d97d6f 216 b0 = *pCoeffs++;
emh203 0:3d9c67d97d6f 217 b1 = *pCoeffs++;
emh203 0:3d9c67d97d6f 218 b2 = *pCoeffs++;
emh203 0:3d9c67d97d6f 219 a1 = *pCoeffs++;
emh203 0:3d9c67d97d6f 220 a2 = *pCoeffs++;
emh203 0:3d9c67d97d6f 221
emh203 0:3d9c67d97d6f 222 /* Reading the state values */
emh203 0:3d9c67d97d6f 223 Xn1 = (q31_t) (pState[0]);
emh203 0:3d9c67d97d6f 224 Xn2 = (q31_t) (pState[1]);
emh203 0:3d9c67d97d6f 225 Yn1 = pState[2];
emh203 0:3d9c67d97d6f 226 Yn2 = pState[3];
emh203 0:3d9c67d97d6f 227
emh203 0:3d9c67d97d6f 228 /* Apply loop unrolling and compute 4 output values simultaneously. */
emh203 0:3d9c67d97d6f 229 /* The variable acc hold output value that is being computed and
emh203 0:3d9c67d97d6f 230 * stored in the destination buffer
emh203 0:3d9c67d97d6f 231 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
emh203 0:3d9c67d97d6f 232 */
emh203 0:3d9c67d97d6f 233
emh203 0:3d9c67d97d6f 234 sample = blockSize >> 2u;
emh203 0:3d9c67d97d6f 235
emh203 0:3d9c67d97d6f 236 /* First part of the processing with loop unrolling. Compute 4 outputs at a time.
emh203 0:3d9c67d97d6f 237 ** a second loop below computes the remaining 1 to 3 samples. */
emh203 0:3d9c67d97d6f 238 while(sample > 0u)
emh203 0:3d9c67d97d6f 239 {
emh203 0:3d9c67d97d6f 240 /* Read the input */
emh203 0:3d9c67d97d6f 241 Xn = *pIn++;
emh203 0:3d9c67d97d6f 242
emh203 0:3d9c67d97d6f 243 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 244
emh203 0:3d9c67d97d6f 245 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 246 acc = (q63_t) Xn *b0;
emh203 0:3d9c67d97d6f 247
emh203 0:3d9c67d97d6f 248 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 249 acc += (q63_t) Xn1 *b1;
emh203 0:3d9c67d97d6f 250
emh203 0:3d9c67d97d6f 251 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 252 acc += (q63_t) Xn2 *b2;
emh203 0:3d9c67d97d6f 253
emh203 0:3d9c67d97d6f 254 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 255 acc += mult32x64(Yn1, a1);
emh203 0:3d9c67d97d6f 256
emh203 0:3d9c67d97d6f 257 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 258 acc += mult32x64(Yn2, a2);
emh203 0:3d9c67d97d6f 259
emh203 0:3d9c67d97d6f 260 /* The result is converted to 1.63 , Yn2 variable is reused */
emh203 0:3d9c67d97d6f 261 Yn2 = acc << shift;
emh203 0:3d9c67d97d6f 262
emh203 0:3d9c67d97d6f 263 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 264 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 265
emh203 0:3d9c67d97d6f 266 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 267 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 268
emh203 0:3d9c67d97d6f 269 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 270 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 271
emh203 0:3d9c67d97d6f 272 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 273 *pOut = acc_h;
emh203 0:3d9c67d97d6f 274
emh203 0:3d9c67d97d6f 275 /* Read the second input into Xn2, to reuse the value */
emh203 0:3d9c67d97d6f 276 Xn2 = *pIn++;
emh203 0:3d9c67d97d6f 277
emh203 0:3d9c67d97d6f 278 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 279
emh203 0:3d9c67d97d6f 280 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 281 acc = (q63_t) Xn *b1;
emh203 0:3d9c67d97d6f 282
emh203 0:3d9c67d97d6f 283 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 284 acc += (q63_t) Xn2 *b0;
emh203 0:3d9c67d97d6f 285
emh203 0:3d9c67d97d6f 286 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 287 acc += (q63_t) Xn1 *b2;
emh203 0:3d9c67d97d6f 288
emh203 0:3d9c67d97d6f 289 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 290 acc += mult32x64(Yn2, a1);
emh203 0:3d9c67d97d6f 291
emh203 0:3d9c67d97d6f 292 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 293 acc += mult32x64(Yn1, a2);
emh203 0:3d9c67d97d6f 294
emh203 0:3d9c67d97d6f 295 /* The result is converted to 1.63, Yn1 variable is reused */
emh203 0:3d9c67d97d6f 296 Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 297
emh203 0:3d9c67d97d6f 298 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 299 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 300
emh203 0:3d9c67d97d6f 301 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 302 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 303
emh203 0:3d9c67d97d6f 304 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 305 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 306
emh203 0:3d9c67d97d6f 307 /* Read the third input into Xn1, to reuse the value */
emh203 0:3d9c67d97d6f 308 Xn1 = *pIn++;
emh203 0:3d9c67d97d6f 309
emh203 0:3d9c67d97d6f 310 /* The result is converted to 1.31 */
emh203 0:3d9c67d97d6f 311 /* Store the output in the destination buffer. */
emh203 0:3d9c67d97d6f 312 *(pOut + 1u) = acc_h;
emh203 0:3d9c67d97d6f 313
emh203 0:3d9c67d97d6f 314 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 315
emh203 0:3d9c67d97d6f 316 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 317 acc = (q63_t) Xn1 *b0;
emh203 0:3d9c67d97d6f 318
emh203 0:3d9c67d97d6f 319 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 320 acc += (q63_t) Xn2 *b1;
emh203 0:3d9c67d97d6f 321
emh203 0:3d9c67d97d6f 322 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 323 acc += (q63_t) Xn *b2;
emh203 0:3d9c67d97d6f 324
emh203 0:3d9c67d97d6f 325 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 326 acc += mult32x64(Yn1, a1);
emh203 0:3d9c67d97d6f 327
emh203 0:3d9c67d97d6f 328 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 329 acc += mult32x64(Yn2, a2);
emh203 0:3d9c67d97d6f 330
emh203 0:3d9c67d97d6f 331 /* The result is converted to 1.63, Yn2 variable is reused */
emh203 0:3d9c67d97d6f 332 Yn2 = acc << shift;
emh203 0:3d9c67d97d6f 333
emh203 0:3d9c67d97d6f 334 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 335 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 336
emh203 0:3d9c67d97d6f 337 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 338 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 339
emh203 0:3d9c67d97d6f 340 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 341 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 342
emh203 0:3d9c67d97d6f 343 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 344 *(pOut + 2u) = acc_h;
emh203 0:3d9c67d97d6f 345
emh203 0:3d9c67d97d6f 346 /* Read the fourth input into Xn, to reuse the value */
emh203 0:3d9c67d97d6f 347 Xn = *pIn++;
emh203 0:3d9c67d97d6f 348
emh203 0:3d9c67d97d6f 349 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 350 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 351 acc = (q63_t) Xn *b0;
emh203 0:3d9c67d97d6f 352
emh203 0:3d9c67d97d6f 353 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 354 acc += (q63_t) Xn1 *b1;
emh203 0:3d9c67d97d6f 355
emh203 0:3d9c67d97d6f 356 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 357 acc += (q63_t) Xn2 *b2;
emh203 0:3d9c67d97d6f 358
emh203 0:3d9c67d97d6f 359 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 360 acc += mult32x64(Yn2, a1);
emh203 0:3d9c67d97d6f 361
emh203 0:3d9c67d97d6f 362 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 363 acc += mult32x64(Yn1, a2);
emh203 0:3d9c67d97d6f 364
emh203 0:3d9c67d97d6f 365 /* The result is converted to 1.63, Yn1 variable is reused */
emh203 0:3d9c67d97d6f 366 Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 367
emh203 0:3d9c67d97d6f 368 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 369 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 370
emh203 0:3d9c67d97d6f 371 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 372 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 373
emh203 0:3d9c67d97d6f 374 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 375 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 376
emh203 0:3d9c67d97d6f 377 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 378 *(pOut + 3u) = acc_h;
emh203 0:3d9c67d97d6f 379
emh203 0:3d9c67d97d6f 380 /* Every time after the output is computed state should be updated. */
emh203 0:3d9c67d97d6f 381 /* The states should be updated as: */
emh203 0:3d9c67d97d6f 382 /* Xn2 = Xn1 */
emh203 0:3d9c67d97d6f 383 /* Xn1 = Xn */
emh203 0:3d9c67d97d6f 384 /* Yn2 = Yn1 */
emh203 0:3d9c67d97d6f 385 /* Yn1 = acc */
emh203 0:3d9c67d97d6f 386 Xn2 = Xn1;
emh203 0:3d9c67d97d6f 387 Xn1 = Xn;
emh203 0:3d9c67d97d6f 388
emh203 0:3d9c67d97d6f 389 /* update output pointer */
emh203 0:3d9c67d97d6f 390 pOut += 4u;
emh203 0:3d9c67d97d6f 391
emh203 0:3d9c67d97d6f 392 /* decrement the loop counter */
emh203 0:3d9c67d97d6f 393 sample--;
emh203 0:3d9c67d97d6f 394 }
emh203 0:3d9c67d97d6f 395
emh203 0:3d9c67d97d6f 396 /* If the blockSize is not a multiple of 4, compute any remaining output samples here.
emh203 0:3d9c67d97d6f 397 ** No loop unrolling is used. */
emh203 0:3d9c67d97d6f 398 sample = (blockSize & 0x3u);
emh203 0:3d9c67d97d6f 399
emh203 0:3d9c67d97d6f 400 while(sample > 0u)
emh203 0:3d9c67d97d6f 401 {
emh203 0:3d9c67d97d6f 402 /* Read the input */
emh203 0:3d9c67d97d6f 403 Xn = *pIn++;
emh203 0:3d9c67d97d6f 404
emh203 0:3d9c67d97d6f 405 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 406
emh203 0:3d9c67d97d6f 407 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 408 acc = (q63_t) Xn *b0;
emh203 0:3d9c67d97d6f 409 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 410 acc += (q63_t) Xn1 *b1;
emh203 0:3d9c67d97d6f 411 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 412 acc += (q63_t) Xn2 *b2;
emh203 0:3d9c67d97d6f 413 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 414 acc += mult32x64(Yn1, a1);
emh203 0:3d9c67d97d6f 415 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 416 acc += mult32x64(Yn2, a2);
emh203 0:3d9c67d97d6f 417
emh203 0:3d9c67d97d6f 418 /* Every time after the output is computed state should be updated. */
emh203 0:3d9c67d97d6f 419 /* The states should be updated as: */
emh203 0:3d9c67d97d6f 420 /* Xn2 = Xn1 */
emh203 0:3d9c67d97d6f 421 /* Xn1 = Xn */
emh203 0:3d9c67d97d6f 422 /* Yn2 = Yn1 */
emh203 0:3d9c67d97d6f 423 /* Yn1 = acc */
emh203 0:3d9c67d97d6f 424 Xn2 = Xn1;
emh203 0:3d9c67d97d6f 425 Xn1 = Xn;
emh203 0:3d9c67d97d6f 426 Yn2 = Yn1;
emh203 0:3d9c67d97d6f 427 /* The result is converted to 1.63, Yn1 variable is reused */
emh203 0:3d9c67d97d6f 428 Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 429
emh203 0:3d9c67d97d6f 430 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 431 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 432
emh203 0:3d9c67d97d6f 433 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 434 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 435
emh203 0:3d9c67d97d6f 436 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 437 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 438
emh203 0:3d9c67d97d6f 439 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 440 *pOut++ = acc_h;
emh203 0:3d9c67d97d6f 441 //Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 442
emh203 0:3d9c67d97d6f 443 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 444 // *pOut++ = (q31_t) (acc >> (32 - shift));
emh203 0:3d9c67d97d6f 445
emh203 0:3d9c67d97d6f 446 /* decrement the loop counter */
emh203 0:3d9c67d97d6f 447 sample--;
emh203 0:3d9c67d97d6f 448 }
emh203 0:3d9c67d97d6f 449
emh203 0:3d9c67d97d6f 450 /* The first stage output is given as input to the second stage. */
emh203 0:3d9c67d97d6f 451 pIn = pDst;
emh203 0:3d9c67d97d6f 452
emh203 0:3d9c67d97d6f 453 /* Reset to destination buffer working pointer */
emh203 0:3d9c67d97d6f 454 pOut = pDst;
emh203 0:3d9c67d97d6f 455
emh203 0:3d9c67d97d6f 456 /* Store the updated state variables back into the pState array */
emh203 0:3d9c67d97d6f 457 /* Store the updated state variables back into the pState array */
emh203 0:3d9c67d97d6f 458 *pState++ = (q63_t) Xn1;
emh203 0:3d9c67d97d6f 459 *pState++ = (q63_t) Xn2;
emh203 0:3d9c67d97d6f 460 *pState++ = Yn1;
emh203 0:3d9c67d97d6f 461 *pState++ = Yn2;
emh203 0:3d9c67d97d6f 462
emh203 0:3d9c67d97d6f 463 } while(--stage);
emh203 0:3d9c67d97d6f 464
emh203 0:3d9c67d97d6f 465 #else
emh203 0:3d9c67d97d6f 466
emh203 0:3d9c67d97d6f 467 /* Run the below code for Cortex-M0 */
emh203 0:3d9c67d97d6f 468
emh203 0:3d9c67d97d6f 469 do
emh203 0:3d9c67d97d6f 470 {
emh203 0:3d9c67d97d6f 471 /* Reading the coefficients */
emh203 0:3d9c67d97d6f 472 b0 = *pCoeffs++;
emh203 0:3d9c67d97d6f 473 b1 = *pCoeffs++;
emh203 0:3d9c67d97d6f 474 b2 = *pCoeffs++;
emh203 0:3d9c67d97d6f 475 a1 = *pCoeffs++;
emh203 0:3d9c67d97d6f 476 a2 = *pCoeffs++;
emh203 0:3d9c67d97d6f 477
emh203 0:3d9c67d97d6f 478 /* Reading the state values */
emh203 0:3d9c67d97d6f 479 Xn1 = pState[0];
emh203 0:3d9c67d97d6f 480 Xn2 = pState[1];
emh203 0:3d9c67d97d6f 481 Yn1 = pState[2];
emh203 0:3d9c67d97d6f 482 Yn2 = pState[3];
emh203 0:3d9c67d97d6f 483
emh203 0:3d9c67d97d6f 484 /* The variable acc hold output value that is being computed and
emh203 0:3d9c67d97d6f 485 * stored in the destination buffer
emh203 0:3d9c67d97d6f 486 * acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
emh203 0:3d9c67d97d6f 487 */
emh203 0:3d9c67d97d6f 488
emh203 0:3d9c67d97d6f 489 sample = blockSize;
emh203 0:3d9c67d97d6f 490
emh203 0:3d9c67d97d6f 491 while(sample > 0u)
emh203 0:3d9c67d97d6f 492 {
emh203 0:3d9c67d97d6f 493 /* Read the input */
emh203 0:3d9c67d97d6f 494 Xn = *pIn++;
emh203 0:3d9c67d97d6f 495
emh203 0:3d9c67d97d6f 496 /* acc = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */
emh203 0:3d9c67d97d6f 497 /* acc = b0 * x[n] */
emh203 0:3d9c67d97d6f 498 acc = (q63_t) Xn *b0;
emh203 0:3d9c67d97d6f 499 /* acc += b1 * x[n-1] */
emh203 0:3d9c67d97d6f 500 acc += (q63_t) Xn1 *b1;
emh203 0:3d9c67d97d6f 501 /* acc += b[2] * x[n-2] */
emh203 0:3d9c67d97d6f 502 acc += (q63_t) Xn2 *b2;
emh203 0:3d9c67d97d6f 503 /* acc += a1 * y[n-1] */
emh203 0:3d9c67d97d6f 504 acc += mult32x64(Yn1, a1);
emh203 0:3d9c67d97d6f 505 /* acc += a2 * y[n-2] */
emh203 0:3d9c67d97d6f 506 acc += mult32x64(Yn2, a2);
emh203 0:3d9c67d97d6f 507
emh203 0:3d9c67d97d6f 508 /* Every time after the output is computed state should be updated. */
emh203 0:3d9c67d97d6f 509 /* The states should be updated as: */
emh203 0:3d9c67d97d6f 510 /* Xn2 = Xn1 */
emh203 0:3d9c67d97d6f 511 /* Xn1 = Xn */
emh203 0:3d9c67d97d6f 512 /* Yn2 = Yn1 */
emh203 0:3d9c67d97d6f 513 /* Yn1 = acc */
emh203 0:3d9c67d97d6f 514 Xn2 = Xn1;
emh203 0:3d9c67d97d6f 515 Xn1 = Xn;
emh203 0:3d9c67d97d6f 516 Yn2 = Yn1;
emh203 0:3d9c67d97d6f 517
emh203 0:3d9c67d97d6f 518 /* The result is converted to 1.63, Yn1 variable is reused */
emh203 0:3d9c67d97d6f 519 Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 520
emh203 0:3d9c67d97d6f 521 /* Calc lower part of acc */
emh203 0:3d9c67d97d6f 522 acc_l = acc & 0xffffffff;
emh203 0:3d9c67d97d6f 523
emh203 0:3d9c67d97d6f 524 /* Calc upper part of acc */
emh203 0:3d9c67d97d6f 525 acc_h = (acc >> 32) & 0xffffffff;
emh203 0:3d9c67d97d6f 526
emh203 0:3d9c67d97d6f 527 /* Apply shift for lower part of acc and upper part of acc */
emh203 0:3d9c67d97d6f 528 acc_h = (uint32_t) acc_l >> lShift | acc_h << uShift;
emh203 0:3d9c67d97d6f 529
emh203 0:3d9c67d97d6f 530 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 531 *pOut++ = acc_h;
emh203 0:3d9c67d97d6f 532
emh203 0:3d9c67d97d6f 533 //Yn1 = acc << shift;
emh203 0:3d9c67d97d6f 534
emh203 0:3d9c67d97d6f 535 /* Store the output in the destination buffer in 1.31 format. */
emh203 0:3d9c67d97d6f 536 //*pOut++ = (q31_t) (acc >> (32 - shift));
emh203 0:3d9c67d97d6f 537
emh203 0:3d9c67d97d6f 538 /* decrement the loop counter */
emh203 0:3d9c67d97d6f 539 sample--;
emh203 0:3d9c67d97d6f 540 }
emh203 0:3d9c67d97d6f 541
emh203 0:3d9c67d97d6f 542 /* The first stage output is given as input to the second stage. */
emh203 0:3d9c67d97d6f 543 pIn = pDst;
emh203 0:3d9c67d97d6f 544
emh203 0:3d9c67d97d6f 545 /* Reset to destination buffer working pointer */
emh203 0:3d9c67d97d6f 546 pOut = pDst;
emh203 0:3d9c67d97d6f 547
emh203 0:3d9c67d97d6f 548 /* Store the updated state variables back into the pState array */
emh203 0:3d9c67d97d6f 549 *pState++ = (q63_t) Xn1;
emh203 0:3d9c67d97d6f 550 *pState++ = (q63_t) Xn2;
emh203 0:3d9c67d97d6f 551 *pState++ = Yn1;
emh203 0:3d9c67d97d6f 552 *pState++ = Yn2;
emh203 0:3d9c67d97d6f 553
emh203 0:3d9c67d97d6f 554 } while(--stage);
emh203 0:3d9c67d97d6f 555
emh203 0:3d9c67d97d6f 556 #endif /* #ifndef ARM_MATH_CM0_FAMILY */
emh203 0:3d9c67d97d6f 557 }
emh203 0:3d9c67d97d6f 558
emh203 0:3d9c67d97d6f 559 /**
emh203 0:3d9c67d97d6f 560 * @} end of BiquadCascadeDF1_32x64 group
emh203 0:3d9c67d97d6f 561 */