Subversion Repositories FuelGauge

Rev

Blame | Last modification | View Log | Download | RSS feed

  1. #include "ref.h"
  2.  
  3. void ref_rms_f32(
  4.   float32_t * pSrc,
  5.   uint32_t blockSize,
  6.   float32_t * pResult)
  7. {
  8.         uint32_t i;
  9.         float32_t sumsq=0;
  10.        
  11.         for(i=0;i<blockSize;i++)
  12.         {
  13.                         sumsq += pSrc[i] * pSrc[i];
  14.         }
  15.         *pResult = sqrtf(sumsq / (float32_t)blockSize);
  16. }
  17.  
  18. void ref_rms_q31(
  19.   q31_t * pSrc,
  20.   uint32_t blockSize,
  21.   q31_t * pResult)
  22. {
  23.         uint32_t i;
  24.         q63_t sumsq=0;
  25.         q63_t tmp1;
  26.         q31_t tmp2;
  27.  
  28.   float help_float;
  29.  
  30.         for(i=0;i<blockSize;i++)
  31.         {
  32.                         sumsq += (q63_t)pSrc[i] * pSrc[i];
  33.         }
  34.         tmp1 = (sumsq / (q63_t)blockSize) >> 31;
  35.         tmp2 = ref_sat_q31(tmp1);
  36.  
  37.   /* GCC M0 problem: __aeabi_f2iz(QNAN) returns not 0 */
  38.   help_float = (sqrtf((float)tmp2 / 2147483648.0f) * 2147483648.0f);
  39.   /* Checking for a NAN value in help_float */
  40.   if (((*((int *)(&help_float))) & 0x7FC00000) == 0x7FC00000) {
  41.       help_float = 0;
  42.   }
  43.   *pResult = (q31_t)(help_float);
  44.  
  45. //  *pResult = (q31_t)(sqrtf((float)tmp2 / 2147483648.0f) * 2147483648.0f);
  46. }
  47.  
  48. void ref_rms_q15(
  49.   q15_t * pSrc,
  50.   uint32_t blockSize,
  51.   q15_t * pResult)
  52. {
  53.         uint32_t i;
  54.         q63_t sumsq=0;
  55.         q31_t tmp1;
  56.         q15_t tmp2;
  57.        
  58.         for(i=0;i<blockSize;i++)
  59.         {
  60.                         sumsq += (q63_t)pSrc[i] * pSrc[i];
  61.         }
  62.         tmp1 = (sumsq / (q63_t)blockSize) >> 15;
  63.         tmp2 = ref_sat_q15(tmp1);
  64.         *pResult = (q15_t)(sqrtf((float)tmp2 / 32768.0f) * 32768.0f);
  65. }
  66.