Subversion Repositories AFRtranscoder

Rev

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

  1. /* ----------------------------------------------------------------------
  2. * Copyright (C) 2010 ARM Limited. All rights reserved.
  3. *
  4. * $Date:        29. November 2010
  5. * $Revision:    V1.0.3
  6. *
  7. * Project:      CMSIS DSP Library
  8. *
  9. * Title:        math_helper.c
  10. *
  11. * Description:  Definition of all helper functions required.
  12. *
  13. * Target Processor: Cortex-M4/Cortex-M3
  14. *
  15. * Version 1.0.3 2010/11/29
  16. *    Re-organized the CMSIS folders and updated documentation.
  17. *
  18. * Version 1.0.2 2010/11/11
  19. *    Documentation updated.
  20. *
  21. * Version 1.0.1 2010/10/05
  22. *    Production release and review comments incorporated.
  23. *
  24. * Version 1.0.0 2010/09/20
  25. *    Production release and review comments incorporated.
  26. *
  27. * Version 0.0.7  2010/06/10
  28. *    Misra-C changes done
  29. * -------------------------------------------------------------------- */
  30.  
  31. /* ----------------------------------------------------------------------
  32. *       Include standard header files
  33. * -------------------------------------------------------------------- */
  34. #include<math.h>
  35.  
  36. /* ----------------------------------------------------------------------
  37. *       Include project header files
  38. * -------------------------------------------------------------------- */
  39. #include "math_helper.h"
  40.  
  41. /**
  42.  * @brief  Caluclation of SNR
  43.  * @param  float*   Pointer to the reference buffer
  44.  * @param  float*   Pointer to the test buffer
  45.  * @param  uint32_t     total number of samples
  46.  * @return float    SNR
  47.  * The function Caluclates signal to noise ratio for the reference output
  48.  * and test output
  49.  */
  50.  
  51. float arm_snr_f32(float *pRef, float *pTest, uint32_t buffSize)
  52. {
  53.   float EnergySignal = 0.0, EnergyError = 0.0;
  54.   uint32_t i;
  55.   float SNR;
  56.   int temp;
  57.   int *test;
  58.  
  59.   for (i = 0; i < buffSize; i++)
  60.     {
  61.       /* Checking for a NAN value in pRef array */
  62.       test =   (int *)(&pRef[i]);
  63.       temp =  *test;
  64.  
  65.       if (temp == 0x7FC00000)
  66.       {
  67.             return(0);
  68.       }
  69.  
  70.       /* Checking for a NAN value in pTest array */
  71.       test =   (int *)(&pTest[i]);
  72.       temp =  *test;
  73.  
  74.       if (temp == 0x7FC00000)
  75.       {
  76.             return(0);
  77.       }
  78.       EnergySignal += pRef[i] * pRef[i];
  79.       EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]);
  80.     }
  81.  
  82.     /* Checking for a NAN value in EnergyError */
  83.     test =   (int *)(&EnergyError);
  84.     temp =  *test;
  85.  
  86.     if (temp == 0x7FC00000)
  87.     {
  88.         return(0);
  89.     }
  90.  
  91.  
  92.   SNR = 10 * log10f (EnergySignal / EnergyError);
  93.  
  94.         return (SNR);
  95.  
  96. }
  97.  
  98.  
  99.  
  100. double arm_snr_f64(double *pRef, double *pTest, uint32_t buffSize)
  101. {
  102.   double EnergySignal = 0.0, EnergyError = 0.0;
  103.   uint32_t i;
  104.   double SNR;
  105.   int temp;
  106.   int *test;
  107.  
  108.   for (i = 0; i < buffSize; i++)
  109.     {
  110.       /* Checking for a NAN value in pRef array */
  111.       test =   (int *)(&pRef[i]);
  112.       temp =  *test;
  113.  
  114.       if (temp == 0x7FC00000)
  115.       {
  116.             return(0);
  117.       }
  118.  
  119.       /* Checking for a NAN value in pTest array */
  120.       test =   (int *)(&pTest[i]);
  121.       temp =  *test;
  122.  
  123.       if (temp == 0x7FC00000)
  124.       {
  125.             return(0);
  126.       }
  127.       EnergySignal += pRef[i] * pRef[i];
  128.       EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]);
  129.     }
  130.  
  131.     /* Checking for a NAN value in EnergyError */
  132.     test =   (int *)(&EnergyError);
  133.     temp =  *test;
  134.  
  135.     if (temp == 0x7FC00000)
  136.     {
  137.         return(0);
  138.     }
  139.  
  140.  
  141.   SNR = 10 * log10 (EnergySignal / EnergyError);
  142.  
  143.   return (SNR);
  144.  
  145. }
  146.  
  147. /**
  148.  * @brief  Provide guard bits for Input buffer
  149.  * @param  q15_t*       Pointer to input buffer
  150.  * @param  uint32_t     blockSize
  151.  * @param  uint32_t     guard_bits
  152.  * @return none
  153.  * The function Provides the guard bits for the buffer
  154.  * to avoid overflow
  155.  */
  156.  
  157. void arm_provide_guard_bits_q15 (q15_t * input_buf, uint32_t blockSize,
  158.                             uint32_t guard_bits)
  159. {
  160.   uint32_t i;
  161.  
  162.   for (i = 0; i < blockSize; i++)
  163.     {
  164.       input_buf[i] = input_buf[i] >> guard_bits;
  165.     }
  166. }
  167.  
  168. /**
  169.  * @brief  Converts float to fixed in q12.20 format
  170.  * @param  uint32_t     number of samples in the buffer
  171.  * @return none
  172.  * The function converts floating point values to fixed point(q12.20) values
  173.  */
  174.  
  175. void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples)
  176. {
  177.   uint32_t i;
  178.  
  179.   for (i = 0; i < numSamples; i++)
  180.     {
  181.       /* 1048576.0f corresponds to pow(2, 20) */
  182.       pOut[i] = (q31_t) (pIn[i] * 1048576.0f);
  183.  
  184.       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
  185.  
  186.       if (pIn[i] == (float) 1.0)
  187.         {
  188.           pOut[i] = 0x000FFFFF;
  189.         }
  190.     }
  191. }
  192.  
  193. /**
  194.  * @brief  Compare MATLAB Reference Output and ARM Test output
  195.  * @param  q15_t*   Pointer to Ref buffer
  196.  * @param  q15_t*   Pointer to Test buffer
  197.  * @param  uint32_t     number of samples in the buffer
  198.  * @return none
  199.  */
  200.  
  201. uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t * pOut, uint32_t numSamples)
  202. {
  203.   uint32_t i;
  204.   int32_t diff, diffCrnt = 0;
  205.   uint32_t maxDiff = 0;
  206.  
  207.   for (i = 0; i < numSamples; i++)
  208.   {
  209.     diff = pIn[i] - pOut[i];
  210.     diffCrnt = (diff > 0) ? diff : -diff;
  211.  
  212.     if (diffCrnt > maxDiff)
  213.     {
  214.         maxDiff = diffCrnt;
  215.     }
  216.   }
  217.  
  218.   return(maxDiff);
  219. }
  220.  
  221. /**
  222.  * @brief  Compare MATLAB Reference Output and ARM Test output
  223.  * @param  q31_t*   Pointer to Ref buffer
  224.  * @param  q31_t*   Pointer to Test buffer
  225.  * @param  uint32_t     number of samples in the buffer
  226.  * @return none
  227.  */
  228.  
  229. uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t * pOut, uint32_t numSamples)
  230. {
  231.   uint32_t i;
  232.   int32_t diff, diffCrnt = 0;
  233.   uint32_t maxDiff = 0;
  234.  
  235.   for (i = 0; i < numSamples; i++)
  236.   {
  237.     diff = pIn[i] - pOut[i];
  238.     diffCrnt = (diff > 0) ? diff : -diff;
  239.  
  240.     if (diffCrnt > maxDiff)
  241.     {
  242.         maxDiff = diffCrnt;
  243.     }
  244.   }
  245.  
  246.   return(maxDiff);
  247. }
  248.  
  249. /**
  250.  * @brief  Provide guard bits for Input buffer
  251.  * @param  q31_t*   Pointer to input buffer
  252.  * @param  uint32_t     blockSize
  253.  * @param  uint32_t     guard_bits
  254.  * @return none
  255.  * The function Provides the guard bits for the buffer
  256.  * to avoid overflow
  257.  */
  258.  
  259. void arm_provide_guard_bits_q31 (q31_t * input_buf,
  260.                                  uint32_t blockSize,
  261.                                  uint32_t guard_bits)
  262. {
  263.   uint32_t i;
  264.  
  265.   for (i = 0; i < blockSize; i++)
  266.     {
  267.       input_buf[i] = input_buf[i] >> guard_bits;
  268.     }
  269. }
  270.  
  271. /**
  272.  * @brief  Provide guard bits for Input buffer
  273.  * @param  q31_t*   Pointer to input buffer
  274.  * @param  uint32_t     blockSize
  275.  * @param  uint32_t     guard_bits
  276.  * @return none
  277.  * The function Provides the guard bits for the buffer
  278.  * to avoid overflow
  279.  */
  280.  
  281. void arm_provide_guard_bits_q7 (q7_t * input_buf,
  282.                                 uint32_t blockSize,
  283.                                 uint32_t guard_bits)
  284. {
  285.   uint32_t i;
  286.  
  287.   for (i = 0; i < blockSize; i++)
  288.     {
  289.       input_buf[i] = input_buf[i] >> guard_bits;
  290.     }
  291. }
  292.  
  293.  
  294.  
  295. /**
  296.  * @brief  Caluclates number of guard bits
  297.  * @param  uint32_t     number of additions
  298.  * @return none
  299.  * The function Caluclates the number of guard bits
  300.  * depending on the numtaps
  301.  */
  302.  
  303. uint32_t arm_calc_guard_bits (uint32_t num_adds)
  304. {
  305.   uint32_t i = 1, j = 0;
  306.  
  307.   if (num_adds == 1)
  308.     {
  309.       return (0);
  310.     }
  311.  
  312.   while (i < num_adds)
  313.     {
  314.       i = i * 2;
  315.       j++;
  316.     }
  317.  
  318.   return (j);
  319. }
  320.  
  321. /**
  322.  * @brief  Converts Q15 to floating-point
  323.  * @param  uint32_t     number of samples in the buffer
  324.  * @return none
  325.  */
  326.  
  327. void arm_apply_guard_bits (float32_t * pIn,
  328.                            uint32_t numSamples,
  329.                            uint32_t guard_bits)
  330. {
  331.   uint32_t i;
  332.  
  333.   for (i = 0; i < numSamples; i++)
  334.     {
  335.       pIn[i] = pIn[i] * arm_calc_2pow(guard_bits);
  336.     }
  337. }
  338.  
  339. /**
  340.  * @brief  Calculates pow(2, numShifts)
  341.  * @param  uint32_t     number of shifts
  342.  * @return pow(2, numShifts)
  343.  */
  344. uint32_t arm_calc_2pow(uint32_t numShifts)
  345. {
  346.  
  347.   uint32_t i, val = 1;
  348.  
  349.   for (i = 0; i < numShifts; i++)
  350.     {
  351.       val = val * 2;
  352.     }
  353.  
  354.   return(val);
  355. }
  356.  
  357.  
  358.  
  359. /**
  360.  * @brief  Converts float to fixed q14
  361.  * @param  uint32_t     number of samples in the buffer
  362.  * @return none
  363.  * The function converts floating point values to fixed point values
  364.  */
  365.  
  366. void arm_float_to_q14 (float *pIn, q15_t * pOut,
  367.                        uint32_t numSamples)
  368. {
  369.   uint32_t i;
  370.  
  371.   for (i = 0; i < numSamples; i++)
  372.     {
  373.       /* 16384.0f corresponds to pow(2, 14) */
  374.       pOut[i] = (q15_t) (pIn[i] * 16384.0f);
  375.  
  376.       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
  377.  
  378.       if (pIn[i] == (float) 2.0)
  379.         {
  380.           pOut[i] = 0x7FFF;
  381.         }
  382.  
  383.     }
  384.  
  385. }
  386.  
  387.  
  388. /**
  389.  * @brief  Converts float to fixed q30 format
  390.  * @param  uint32_t     number of samples in the buffer
  391.  * @return none
  392.  * The function converts floating point values to fixed point values
  393.  */
  394.  
  395. void arm_float_to_q30 (float *pIn, q31_t * pOut,
  396.                        uint32_t numSamples)
  397. {
  398.   uint32_t i;
  399.  
  400.   for (i = 0; i < numSamples; i++)
  401.     {
  402.       /* 1073741824.0f corresponds to pow(2, 30) */
  403.       pOut[i] = (q31_t) (pIn[i] * 1073741824.0f);
  404.  
  405.       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
  406.  
  407.       if (pIn[i] == (float) 2.0)
  408.         {
  409.           pOut[i] = 0x7FFFFFFF;
  410.         }
  411.     }
  412. }
  413.  
  414. /**
  415.  * @brief  Converts float to fixed q30 format
  416.  * @param  uint32_t     number of samples in the buffer
  417.  * @return none
  418.  * The function converts floating point values to fixed point values
  419.  */
  420.  
  421. void arm_float_to_q29 (float *pIn, q31_t * pOut,
  422.                        uint32_t numSamples)
  423. {
  424.   uint32_t i;
  425.  
  426.   for (i = 0; i < numSamples; i++)
  427.     {
  428.       /* 1073741824.0f corresponds to pow(2, 30) */
  429.       pOut[i] = (q31_t) (pIn[i] * 536870912.0f);
  430.  
  431.       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
  432.  
  433.       if (pIn[i] == (float) 4.0)
  434.         {
  435.           pOut[i] = 0x7FFFFFFF;
  436.         }
  437.     }
  438. }
  439.  
  440.  
  441. /**
  442.  * @brief  Converts float to fixed q28 format
  443.  * @param  uint32_t     number of samples in the buffer
  444.  * @return none
  445.  * The function converts floating point values to fixed point values
  446.  */
  447.  
  448. void arm_float_to_q28 (float *pIn, q31_t * pOut,
  449.                        uint32_t numSamples)
  450. {
  451.   uint32_t i;
  452.  
  453.   for (i = 0; i < numSamples; i++)
  454.     {
  455.     /* 268435456.0f corresponds to pow(2, 28) */
  456.       pOut[i] = (q31_t) (pIn[i] * 268435456.0f);
  457.  
  458.       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
  459.  
  460.       if (pIn[i] == (float) 8.0)
  461.         {
  462.           pOut[i] = 0x7FFFFFFF;
  463.         }
  464.     }
  465. }
  466.  
  467. /**
  468.  * @brief  Clip the float values to +/- 1
  469.  * @param  pIn      input buffer
  470.  * @param  numSamples   number of samples in the buffer
  471.  * @return none
  472.  * The function converts floating point values to fixed point values
  473.  */
  474.  
  475. void arm_clip_f32 (float *pIn, uint32_t numSamples)
  476. {
  477.   uint32_t i;
  478.  
  479.   for (i = 0; i < numSamples; i++)
  480.     {
  481.       if (pIn[i] > 1.0f)
  482.       {
  483.         pIn[i] = 1.0;
  484.       }
  485.       else if ( pIn[i] < -1.0f)
  486.       {
  487.         pIn[i] = -1.0;
  488.       }
  489.  
  490.     }
  491. }
  492.