hoo2
/
STM32F10x_Std_Lib
1
0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
A bundled STM32F10x Std Periph and CMSIS library
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1 Commit
1 Branch
2.1 MiB
 
 
 
 
 
Tree: c21da4388c
Branches Tags
${ item.name }
Create branch ${ searchTerm }
from 'c21da4388c'
${ noResults }
STM32F10x_Std_Lib/CMSIS/DSP_Lib/Source/FilteringFunctions/arm_biquad_cascade_df1_fast...

306 lines
12 KiB
Raw Blame History 

  1. /* ----------------------------------------------------------------------    

  2. * Copyright (C) 2010-2014 ARM Limited. All rights reserved.    

  3. *    

  4. * $Date:        12. March 2014

  5. * $Revision: 	V1.4.4

  6. *    

  7. * Project: 	    CMSIS DSP Library    

  8. * Title:	    arm_biquad_cascade_df1_fast_q31.c    

  9. *    

  10. * Description:	Processing function for the    

  11. *				Q31 Fast Biquad cascade DirectFormI(DF1) filter.    

  12. *    

  13. * Target Processor: Cortex-M4/Cortex-M3

  14. *  

  15. * Redistribution and use in source and binary forms, with or without 

  16. * modification, are permitted provided that the following conditions

  17. * are met:

  18. *   - Redistributions of source code must retain the above copyright

  19. *     notice, this list of conditions and the following disclaimer.

  20. *   - Redistributions in binary form must reproduce the above copyright

  21. *     notice, this list of conditions and the following disclaimer in

  22. *     the documentation and/or other materials provided with the 

  23. *     distribution.

  24. *   - Neither the name of ARM LIMITED nor the names of its contributors

  25. *     may be used to endorse or promote products derived from this

  26. *     software without specific prior written permission.

  27. *

  28. * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS

  29. * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT

  30. * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS

  31. * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 

  32. * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,

  33. * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,

  34. * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;

  35. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER

  36. * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT

  37. * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN

  38. * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

  39. * POSSIBILITY OF SUCH DAMAGE. 

  40. * -------------------------------------------------------------------- */

  41. 

  42. #include "arm_math.h"

  43. 

  44. /**    

  45.  * @ingroup groupFilters    

  46.  */

  47. 

  48. /**    

  49.  * @addtogroup BiquadCascadeDF1    

  50.  * @{    

  51.  */

  52. 

  53. /**    

  54.  * @details    

  55.  *    

  56.  * @param[in]  *S        points to an instance of the Q31 Biquad cascade structure.    

  57.  * @param[in]  *pSrc     points to the block of input data.    

  58.  * @param[out] *pDst     points to the block of output data.    

  59.  * @param[in]  blockSize number of samples to process per call.    

  60.  * @return 	   none.    

  61.  *    

  62.  * <b>Scaling and Overflow Behavior:</b>    

  63.  * \par    

  64.  * This function is optimized for speed at the expense of fixed-point precision and overflow protection.    

  65.  * The result of each 1.31 x 1.31 multiplication is truncated to 2.30 format.    

  66.  * These intermediate results are added to a 2.30 accumulator.    

  67.  * Finally, the accumulator is saturated and converted to a 1.31 result.    

  68.  * 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.    

  69.  * 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    

  70.  * arm_biquad_cascade_df1_init_q31() to initialize filter structure.    

  71.  *    

  72.  * \par    

  73.  * 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.    

  74.  * Use the function <code>arm_biquad_cascade_df1_init_q31()</code> to initialize the filter structure.    

  75.  */

  76. 

  77. void arm_biquad_cascade_df1_fast_q31(

  78.   const arm_biquad_casd_df1_inst_q31 * S,

  79.   q31_t * pSrc,

  80.   q31_t * pDst,

  81.   uint32_t blockSize)

  82. {

  83.   q31_t acc = 0;                                 /*  accumulator                   */

  84.   q31_t Xn1, Xn2, Yn1, Yn2;                      /*  Filter state variables        */

  85.   q31_t b0, b1, b2, a1, a2;                      /*  Filter coefficients           */

  86.   q31_t *pIn = pSrc;                             /*  input pointer initialization  */

  87.   q31_t *pOut = pDst;                            /*  output pointer initialization */

  88.   q31_t *pState = S->pState;                     /*  pState pointer initialization */

  89.   q31_t *pCoeffs = S->pCoeffs;                   /*  coeff pointer initialization  */

  90.   q31_t Xn;                                      /*  temporary input               */

  91.   int32_t shift = (int32_t) S->postShift + 1;    /*  Shift to be applied to the output */

  92.   uint32_t sample, stage = S->numStages;         /*  loop counters                     */

  93. 

  94. 

  95.   do

  96.   {

  97.     /* Reading the coefficients */

  98.     b0 = *pCoeffs++;

  99.     b1 = *pCoeffs++;

  100.     b2 = *pCoeffs++;

  101.     a1 = *pCoeffs++;

  102.     a2 = *pCoeffs++;

  103. 

  104.     /* Reading the state values */

  105.     Xn1 = pState[0];

  106.     Xn2 = pState[1];

  107.     Yn1 = pState[2];

  108.     Yn2 = pState[3];

  109. 

  110.     /* Apply loop unrolling and compute 4 output values simultaneously. */

  111.     /*      The variables acc ... acc3 hold output values that are being computed:       

  112.      *       

  113.      *    acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]       

  114.      */

  115. 

  116.     sample = blockSize >> 2u;

  117. 

  118.     /* First part of the processing with loop unrolling.  Compute 4 outputs at a time.       

  119.      ** a second loop below computes the remaining 1 to 3 samples. */

  120.     while(sample > 0u)

  121.     {

  122.       /* Read the input */

  123.       Xn = *pIn;

  124. 

  125.       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */

  126.       /* acc =  b0 * x[n] */

  127.       //acc = (q31_t) (((q63_t) b1 * Xn1) >> 32);

  128.       mult_32x32_keep32_R(acc, b1, Xn1);

  129.       /* acc +=  b1 * x[n-1] */

  130.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b0 * (Xn))) >> 32);   

  131.       multAcc_32x32_keep32_R(acc, b0, Xn);

  132.       /* acc +=  b[2] * x[n-2] */

  133.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);

  134.       multAcc_32x32_keep32_R(acc, b2, Xn2);

  135.       /* acc +=  a1 * y[n-1] */

  136.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);

  137.       multAcc_32x32_keep32_R(acc, a1, Yn1);

  138.       /* acc +=  a2 * y[n-2] */

  139.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);

  140.       multAcc_32x32_keep32_R(acc, a2, Yn2);

  141. 

  142.       /* The result is converted to 1.31 , Yn2 variable is reused */

  143.       Yn2 = acc << shift;

  144. 

  145.       /* Read the second input */

  146.       Xn2 = *(pIn + 1u);

  147. 

  148.       /* Store the output in the destination buffer. */

  149.       *pOut = Yn2;

  150. 

  151.       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */

  152.       /* acc =  b0 * x[n] */

  153.       //acc = (q31_t) (((q63_t) b0 * (Xn2)) >> 32);

  154.       mult_32x32_keep32_R(acc, b0, Xn2);

  155.       /* acc +=  b1 * x[n-1] */

  156.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn))) >> 32);

  157.       multAcc_32x32_keep32_R(acc, b1, Xn);

  158.       /* acc +=  b[2] * x[n-2] */

  159.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn1))) >> 32);

  160.       multAcc_32x32_keep32_R(acc, b2, Xn1);

  161.       /* acc +=  a1 * y[n-1] */

  162.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);

  163.       multAcc_32x32_keep32_R(acc, a1, Yn2);

  164.       /* acc +=  a2 * y[n-2] */

  165.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);

  166.       multAcc_32x32_keep32_R(acc, a2, Yn1);

  167. 

  168.       /* The result is converted to 1.31, Yn1 variable is reused  */

  169.       Yn1 = acc << shift;

  170. 

  171.       /* Read the third input  */

  172.       Xn1 = *(pIn + 2u);

  173. 

  174.       /* Store the output in the destination buffer. */

  175.       *(pOut + 1u) = Yn1;

  176. 

  177.       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */

  178.       /* acc =  b0 * x[n] */

  179.       //acc = (q31_t) (((q63_t) b0 * (Xn1)) >> 32);

  180.       mult_32x32_keep32_R(acc, b0, Xn1);

  181.       /* acc +=  b1 * x[n-1] */

  182.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn2))) >> 32);

  183.       multAcc_32x32_keep32_R(acc, b1, Xn2);

  184.       /* acc +=  b[2] * x[n-2] */

  185.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn))) >> 32);

  186.       multAcc_32x32_keep32_R(acc, b2, Xn);

  187.       /* acc +=  a1 * y[n-1] */

  188.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);

  189.       multAcc_32x32_keep32_R(acc, a1, Yn1);

  190.       /* acc +=  a2 * y[n-2] */

  191.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);

  192.       multAcc_32x32_keep32_R(acc, a2, Yn2);

  193. 

  194.       /* The result is converted to 1.31, Yn2 variable is reused  */

  195.       Yn2 = acc << shift;

  196. 

  197.       /* Read the forth input */

  198.       Xn = *(pIn + 3u);

  199. 

  200.       /* Store the output in the destination buffer. */

  201.       *(pOut + 2u) = Yn2;

  202.       pIn += 4u;

  203. 

  204.       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */

  205.       /* acc =  b0 * x[n] */

  206.       //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);

  207.       mult_32x32_keep32_R(acc, b0, Xn);

  208.       /* acc +=  b1 * x[n-1] */

  209.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);

  210.       multAcc_32x32_keep32_R(acc, b1, Xn1);

  211.       /* acc +=  b[2] * x[n-2] */

  212.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);

  213.       multAcc_32x32_keep32_R(acc, b2, Xn2);

  214.       /* acc +=  a1 * y[n-1] */

  215.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn2))) >> 32);

  216.       multAcc_32x32_keep32_R(acc, a1, Yn2);

  217.       /* acc +=  a2 * y[n-2] */

  218.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn1))) >> 32);

  219.       multAcc_32x32_keep32_R(acc, a2, Yn1);

  220. 

  221.       /* Every time after the output is computed state should be updated. */

  222.       /* The states should be updated as:  */

  223.       /* Xn2 = Xn1    */

  224.       Xn2 = Xn1;

  225. 

  226.       /* The result is converted to 1.31, Yn1 variable is reused  */

  227.       Yn1 = acc << shift;

  228. 

  229.       /* Xn1 = Xn     */

  230.       Xn1 = Xn;

  231. 

  232.       /* Store the output in the destination buffer. */

  233.       *(pOut + 3u) = Yn1;

  234.       pOut += 4u;

  235. 

  236.       /* decrement the loop counter */

  237.       sample--;

  238.     }

  239. 

  240.     /* If the blockSize is not a multiple of 4, compute any remaining output samples here.       

  241.      ** No loop unrolling is used. */

  242.     sample = (blockSize & 0x3u);

  243. 

  244.    while(sample > 0u)

  245.    {

  246.       /* Read the input */

  247.       Xn = *pIn++;

  248. 

  249.       /* acc =  b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2] */

  250.       /* acc =  b0 * x[n] */

  251.       //acc = (q31_t) (((q63_t) b0 * (Xn)) >> 32);

  252.       mult_32x32_keep32_R(acc, b0, Xn);

  253.       /* acc +=  b1 * x[n-1] */

  254.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b1 * (Xn1))) >> 32);

  255.       multAcc_32x32_keep32_R(acc, b1, Xn1);

  256.       /* acc +=  b[2] * x[n-2] */

  257.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) b2 * (Xn2))) >> 32);

  258.       multAcc_32x32_keep32_R(acc, b2, Xn2);

  259.       /* acc +=  a1 * y[n-1] */

  260.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a1 * (Yn1))) >> 32);

  261.       multAcc_32x32_keep32_R(acc, a1, Yn1);

  262.       /* acc +=  a2 * y[n-2] */

  263.       //acc = (q31_t) ((((q63_t) acc << 32) + ((q63_t) a2 * (Yn2))) >> 32);

  264.       multAcc_32x32_keep32_R(acc, a2, Yn2);

  265. 

  266.       /* The result is converted to 1.31  */

  267.       acc = acc << shift;

  268. 

  269.       /* Every time after the output is computed state should be updated. */

  270.       /* The states should be updated as:  */

  271.       /* Xn2 = Xn1    */

  272.       /* Xn1 = Xn     */

  273.       /* Yn2 = Yn1    */

  274.       /* Yn1 = acc    */

  275.       Xn2 = Xn1;

  276.       Xn1 = Xn;

  277.       Yn2 = Yn1;

  278.       Yn1 = acc;

  279. 

  280.       /* Store the output in the destination buffer. */

  281.       *pOut++ = acc;

  282. 

  283.       /* decrement the loop counter */

  284.       sample--;

  285.    }

  286. 

  287.     /*  The first stage goes from the input buffer to the output buffer. */

  288.     /*  Subsequent stages occur in-place in the output buffer */

  289.     pIn = pDst;

  290. 

  291.     /* Reset to destination pointer */

  292.     pOut = pDst;

  293. 

  294.     /*  Store the updated state variables back into the pState array */

  295.     *pState++ = Xn1;

  296.     *pState++ = Xn2;

  297.     *pState++ = Yn1;

  298.     *pState++ = Yn2;

  299. 

  300.   } while(--stage);

  301. }

  302. 

  303. /**    

  304.   * @} end of BiquadCascadeDF1 group    

  305.   */