Subversion Repositories dashGPS

Rev

Rev 2 | Blame | Compare with Previous | Last modification | View Log | Download | RSS feed

  1. /* ----------------------------------------------------------------------
  2.  * Project:      CMSIS DSP Library
  3.  * Title:        arm_rms_q31.c
  4.  * Description:  Root Mean Square of the elements of a Q31 vector
  5.  *
  6.  * $Date:        27. January 2017
  7.  * $Revision:    V.1.5.1
  8.  *
  9.  * Target Processor: Cortex-M cores
  10.  * -------------------------------------------------------------------- */
  11. /*
  12.  * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
  13.  *
  14.  * SPDX-License-Identifier: Apache-2.0
  15.  *
  16.  * Licensed under the Apache License, Version 2.0 (the License); you may
  17.  * not use this file except in compliance with the License.
  18.  * You may obtain a copy of the License at
  19.  *
  20.  * www.apache.org/licenses/LICENSE-2.0
  21.  *
  22.  * Unless required by applicable law or agreed to in writing, software
  23.  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
  24.  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  25.  * See the License for the specific language governing permissions and
  26.  * limitations under the License.
  27.  */
  28.  
  29. #include "arm_math.h"
  30.  
  31. /**
  32.  * @addtogroup RMS
  33.  * @{
  34.  */
  35.  
  36.  
  37. /**
  38.  * @brief Root Mean Square of the elements of a Q31 vector.
  39.  * @param[in]       *pSrc points to the input vector
  40.  * @param[in]       blockSize length of the input vector
  41.  * @param[out]      *pResult rms value returned here
  42.  * @return none.
  43.  *
  44.  * @details
  45.  * <b>Scaling and Overflow Behavior:</b>
  46.  *
  47.  *\par
  48.  * The function is implemented using an internal 64-bit accumulator.
  49.  * The input is represented in 1.31 format, and intermediate multiplication
  50.  * yields a 2.62 format.
  51.  * The accumulator maintains full precision of the intermediate multiplication results,
  52.  * but provides only a single guard bit.
  53.  * There is no saturation on intermediate additions.
  54.  * If the accumulator overflows, it wraps around and distorts the result.
  55.  * In order to avoid overflows completely, the input signal must be scaled down by
  56.  * log2(blockSize) bits, as a total of blockSize additions are performed internally.
  57.  * Finally, the 2.62 accumulator is right shifted by 31 bits to yield a 1.31 format value.
  58.  *
  59.  */
  60.  
  61. void arm_rms_q31(
  62.   q31_t * pSrc,
  63.   uint32_t blockSize,
  64.   q31_t * pResult)
  65. {
  66.   q63_t sum = 0;                                 /* accumulator */
  67.   q31_t in;                                      /* Temporary variable to store the input */
  68.   uint32_t blkCnt;                               /* loop counter */
  69.  
  70. #if defined (ARM_MATH_DSP)
  71.   /* Run the below code for Cortex-M4 and Cortex-M3 */
  72.  
  73.   q31_t in1, in2, in3, in4;                      /* Temporary input variables */
  74.  
  75.   /*loop Unrolling */
  76.   blkCnt = blockSize >> 2U;
  77.  
  78.   /* First part of the processing with loop unrolling.  Compute 8 outputs at a time.
  79.    ** a second loop below computes the remaining 1 to 7 samples. */
  80.   while (blkCnt > 0U)
  81.   {
  82.     /* C = A[0] * A[0] + A[1] * A[1] + A[2] * A[2] + ... + A[blockSize-1] * A[blockSize-1] */
  83.     /* Compute sum of the squares and then store the result in a temporary variable, sum */
  84.     /* read two samples from source buffer */
  85.     in1 = pSrc[0];
  86.     in2 = pSrc[1];
  87.  
  88.     /* calculate power and accumulate to accumulator */
  89.     sum += (q63_t) in1 *in1;
  90.     sum += (q63_t) in2 *in2;
  91.  
  92.     /* read two samples from source buffer */
  93.     in3 = pSrc[2];
  94.     in4 = pSrc[3];
  95.  
  96.     /* calculate power and accumulate to accumulator */
  97.     sum += (q63_t) in3 *in3;
  98.     sum += (q63_t) in4 *in4;
  99.  
  100.  
  101.     /* update source buffer to process next samples */
  102.     pSrc += 4U;
  103.  
  104.     /* Decrement the loop counter */
  105.     blkCnt--;
  106.   }
  107.  
  108.   /* If the blockSize is not a multiple of 8, compute any remaining output samples here.
  109.    ** No loop unrolling is used. */
  110.   blkCnt = blockSize % 0x4U;
  111.  
  112. #else
  113.   /* Run the below code for Cortex-M0 */
  114.  
  115.   blkCnt = blockSize;
  116.  
  117. #endif /* #if defined (ARM_MATH_DSP) */
  118.  
  119.   while (blkCnt > 0U)
  120.   {
  121.     /* C = A[0] * A[0] + A[1] * A[1] + A[2] * A[2] + ... + A[blockSize-1] * A[blockSize-1] */
  122.     /* Compute sum of the squares and then store the results in a temporary variable, sum */
  123.     in = *pSrc++;
  124.     sum += (q63_t) in *in;
  125.  
  126.     /* Decrement the loop counter */
  127.     blkCnt--;
  128.   }
  129.  
  130.   /* Convert data in 2.62 to 1.31 by 31 right shifts and saturate */
  131.   /* Compute Rms and store the result in the destination vector */
  132.   arm_sqrt_q31(clip_q63_to_q31((sum / (q63_t) blockSize) >> 31), pResult);
  133. }
  134.  
  135. /**
  136.  * @} end of RMS group
  137.  */
  138.