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

287 lines
10 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_q15.c    

  9. *    

  10. * Description:	Fast processing function for the    

  11. *				Q15 Biquad cascade 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.  * @param[in]  *S points to an instance of the Q15 Biquad cascade structure.    

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

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

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

  59.  * @return none.    

  60.  *    

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

  62.  * \par    

  63.  * This fast version uses a 32-bit accumulator with 2.30 format.    

  64.  * The accumulator maintains full precision of the intermediate multiplication results but provides only a single guard bit.    

  65.  * Thus, if the accumulator result overflows it wraps around and distorts the result.    

  66.  * 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).    

  67.  * The 2.30 accumulator is then shifted by <code>postShift</code> bits and the result truncated to 1.15 format by discarding the low 16 bits.    

  68.  *    

  69.  * \par    

  70.  * Refer to the function <code>arm_biquad_cascade_df1_q15()</code> for a slower implementation of this function which uses 64-bit accumulation to avoid wrap around distortion.  Both the slow and the fast versions use the same instance structure.    

  71.  * Use the function <code>arm_biquad_cascade_df1_init_q15()</code> to initialize the filter structure.    

  72.  *    

  73.  */

  74. 

  75. void arm_biquad_cascade_df1_fast_q15(

  76.   const arm_biquad_casd_df1_inst_q15 * S,

  77.   q15_t * pSrc,

  78.   q15_t * pDst,

  79.   uint32_t blockSize)

  80. {

  81.   q15_t *pIn = pSrc;                             /*  Source pointer                               */

  82.   q15_t *pOut = pDst;                            /*  Destination pointer                          */

  83.   q31_t in;                                      /*  Temporary variable to hold input value       */

  84.   q31_t out;                                     /*  Temporary variable to hold output value      */

  85.   q31_t b0;                                      /*  Temporary variable to hold bo value          */

  86.   q31_t b1, a1;                                  /*  Filter coefficients                          */

  87.   q31_t state_in, state_out;                     /*  Filter state variables                       */

  88.   q31_t acc;                                     /*  Accumulator                                  */

  89.   int32_t shift = (int32_t) (15 - S->postShift); /*  Post shift                                   */

  90.   q15_t *pState = S->pState;                     /*  State pointer                                */

  91.   q15_t *pCoeffs = S->pCoeffs;                   /*  Coefficient pointer                          */

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

  93. 

  94. 

  95. 

  96.   do

  97.   {

  98. 

  99.     /* Read the b0 and 0 coefficients using SIMD  */

  100.     b0 = *__SIMD32(pCoeffs)++;

  101. 

  102.     /* Read the b1 and b2 coefficients using SIMD */

  103.     b1 = *__SIMD32(pCoeffs)++;

  104. 

  105.     /* Read the a1 and a2 coefficients using SIMD */

  106.     a1 = *__SIMD32(pCoeffs)++;

  107. 

  108.     /* Read the input state values from the state buffer:  x[n-1], x[n-2] */

  109.     state_in = *__SIMD32(pState)++;

  110. 

  111.     /* Read the output state values from the state buffer:  y[n-1], y[n-2] */

  112.     state_out = *__SIMD32(pState)--;

  113. 

  114.     /* Apply loop unrolling and compute 2 output values simultaneously. */

  115.     /*      The variable acc hold output values that are being computed:       

  116.      *    

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

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

  119.      */

  120.     sample = blockSize >> 1u;

  121. 

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

  123.      ** a second loop below computes the remaining 1 sample. */

  124.     while(sample > 0u)

  125.     {

  126. 

  127.       /* Read the input */

  128.       in = *__SIMD32(pIn)++;

  129. 

  130.       /* out =  b0 * x[n] + 0 * 0 */

  131.       out = __SMUAD(b0, in);

  132.       /* acc =  b1 * x[n-1] + acc +=  b2 * x[n-2] + out */

  133.       acc = __SMLAD(b1, state_in, out);

  134.       /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */

  135.       acc = __SMLAD(a1, state_out, acc);

  136. 

  137.       /* The result is converted from 3.29 to 1.31 and then saturation is applied */

  138.       out = __SSAT((acc >> shift), 16);

  139. 

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

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

  142.       /* Xn2 = Xn1    */

  143.       /* Xn1 = Xn     */

  144.       /* Yn2 = Yn1    */

  145.       /* Yn1 = acc   */

  146.       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */

  147.       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */

  148. 

  149. #ifndef  ARM_MATH_BIG_ENDIAN

  150. 

  151.       state_in = __PKHBT(in, state_in, 16);

  152.       state_out = __PKHBT(out, state_out, 16);

  153. 

  154. #else

  155. 

  156.       state_in = __PKHBT(state_in >> 16, (in >> 16), 16);

  157.       state_out = __PKHBT(state_out >> 16, (out), 16);

  158. 

  159. #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */

  160. 

  161.       /* out =  b0 * x[n] + 0 * 0 */

  162.       out = __SMUADX(b0, in);

  163.       /* acc0 =  b1 * x[n-1] , acc0 +=  b2 * x[n-2] + out */

  164.       acc = __SMLAD(b1, state_in, out);

  165.       /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */

  166.       acc = __SMLAD(a1, state_out, acc);

  167. 

  168.       /* The result is converted from 3.29 to 1.31 and then saturation is applied */

  169.       out = __SSAT((acc >> shift), 16);

  170. 

  171. 

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

  173. 

  174. #ifndef  ARM_MATH_BIG_ENDIAN

  175. 

  176.       *__SIMD32(pOut)++ = __PKHBT(state_out, out, 16);

  177. 

  178. #else

  179. 

  180.       *__SIMD32(pOut)++ = __PKHBT(out, state_out >> 16, 16);

  181. 

  182. #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */

  183. 

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

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

  186.       /* Xn2 = Xn1    */

  187.       /* Xn1 = Xn     */

  188.       /* Yn2 = Yn1    */

  189.       /* Yn1 = acc   */

  190.       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */

  191.       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */

  192. 

  193. #ifndef  ARM_MATH_BIG_ENDIAN

  194. 

  195.       state_in = __PKHBT(in >> 16, state_in, 16);

  196.       state_out = __PKHBT(out, state_out, 16);

  197. 

  198. #else

  199. 

  200.       state_in = __PKHBT(state_in >> 16, in, 16);

  201.       state_out = __PKHBT(state_out >> 16, out, 16);

  202. 

  203. #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */

  204. 

  205. 

  206.       /* Decrement the loop counter */

  207.       sample--;

  208. 

  209.     }

  210. 

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

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

  213. 

  214.     if((blockSize & 0x1u) != 0u)

  215.     {

  216.       /* Read the input */

  217.       in = *pIn++;

  218. 

  219.       /* out =  b0 * x[n] + 0 * 0 */

  220. 

  221. #ifndef  ARM_MATH_BIG_ENDIAN

  222. 

  223.       out = __SMUAD(b0, in);

  224. 

  225. #else

  226. 

  227.       out = __SMUADX(b0, in);

  228. 

  229. #endif /*      #ifndef  ARM_MATH_BIG_ENDIAN    */

  230. 

  231.       /* acc =  b1 * x[n-1], acc +=  b2 * x[n-2] + out */

  232.       acc = __SMLAD(b1, state_in, out);

  233.       /* acc +=  a1 * y[n-1] + acc +=  a2 * y[n-2] */

  234.       acc = __SMLAD(a1, state_out, acc);

  235. 

  236.       /* The result is converted from 3.29 to 1.31 and then saturation is applied */

  237.       out = __SSAT((acc >> shift), 16);

  238. 

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

  240.       *pOut++ = (q15_t) out;

  241. 

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

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

  244.       /* Xn2 = Xn1    */

  245.       /* Xn1 = Xn     */

  246.       /* Yn2 = Yn1    */

  247.       /* Yn1 = acc   */

  248.       /* x[n-N], x[n-N-1] are packed together to make state_in of type q31 */

  249.       /* y[n-N], y[n-N-1] are packed together to make state_out of type q31 */

  250. 

  251. #ifndef  ARM_MATH_BIG_ENDIAN

  252. 

  253.       state_in = __PKHBT(in, state_in, 16);

  254.       state_out = __PKHBT(out, state_out, 16);

  255. 

  256. #else

  257. 

  258.       state_in = __PKHBT(state_in >> 16, in, 16);

  259.       state_out = __PKHBT(state_out >> 16, out, 16);

  260. 

  261. #endif /*   #ifndef  ARM_MATH_BIG_ENDIAN    */

  262. 

  263.     }

  264. 

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

  266.     /*  Subsequent (numStages - 1) occur in-place in the output buffer  */

  267.     pIn = pDst;

  268. 

  269.     /* Reset the output pointer */

  270.     pOut = pDst;

  271. 

  272.     /*  Store the updated state variables back into the state array */

  273.     *__SIMD32(pState)++ = state_in;

  274.     *__SIMD32(pState)++ = state_out;

  275. 

  276. 

  277.     /* Decrement the loop counter */

  278.     stage--;

  279. 

  280.   } while(stage > 0u);

  281. }

  282. 

  283. 

  284. /**    

  285.  * @} end of BiquadCascadeDF1 group    

  286.  */