math_helper.c

00001 /* ----------------------------------------------------------------------   
00002 * Copyright (C) 2010-2012 ARM Limited. All rights reserved.   
00003 *   
00004 * $Date:        17. January 2013  
00005 * $Revision:    V1.4.0    
00006 *  
00007 * Project:      CMSIS DSP Library 
00008 *
00009 * Title:        math_helper.c
00010 *
00011 * Description:  Definition of all helper functions required.  
00012 *  
00013 * Target Processor: Cortex-M4/Cortex-M3
00014 *  
00015 * Redistribution and use in source and binary forms, with or without 
00016 * modification, are permitted provided that the following conditions
00017 * are met:
00018 *   - Redistributions of source code must retain the above copyright
00019 *     notice, this list of conditions and the following disclaimer.
00020 *   - Redistributions in binary form must reproduce the above copyright
00021 *     notice, this list of conditions and the following disclaimer in
00022 *     the documentation and/or other materials provided with the 
00023 *     distribution.
00024 *   - Neither the name of ARM LIMITED nor the names of its contributors
00025 *     may be used to endorse or promote products derived from this
00026 *     software without specific prior written permission.
00027 *
00028 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00029 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00030 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00031 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00032 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00033 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00034 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00035 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00036 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00037 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00038 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00039 * POSSIBILITY OF SUCH DAMAGE.  
00040 * -------------------------------------------------------------------- */
00041 
00042 /* ----------------------------------------------------------------------
00043 *       Include standard header files  
00044 * -------------------------------------------------------------------- */
00045 #include<math.h>
00046 
00047 /* ----------------------------------------------------------------------
00048 *       Include project header files  
00049 * -------------------------------------------------------------------- */
00050 #include "math_helper.h"
00051 
00052 /** 
00053  * @brief  Caluclation of SNR
00054  * @param  float*   Pointer to the reference buffer
00055  * @param  float*   Pointer to the test buffer
00056  * @param  uint32_t total number of samples
00057  * @return float    SNR
00058  * The function Caluclates signal to noise ratio for the reference output 
00059  * and test output 
00060  */
00061 
00062 float arm_snr_f32(float *pRef, float *pTest, uint32_t buffSize)
00063 {
00064   float EnergySignal = 0.0, EnergyError = 0.0;
00065   uint32_t i;
00066   float SNR;
00067   int temp;
00068   int *test;
00069 
00070   for (i = 0; i < buffSize; i++)
00071     {
00072       /* Checking for a NAN value in pRef array */
00073       test =   (int *)(&pRef[i]);
00074       temp =  *test;
00075 
00076       if(temp == 0x7FC00000)
00077       {
00078             return(0);
00079       }
00080 
00081       /* Checking for a NAN value in pTest array */
00082       test =   (int *)(&pTest[i]);
00083       temp =  *test;
00084 
00085       if(temp == 0x7FC00000)
00086       {
00087             return(0);
00088       }
00089       EnergySignal += pRef[i] * pRef[i];
00090       EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]); 
00091     }
00092 
00093     /* Checking for a NAN value in EnergyError */
00094     test =   (int *)(&EnergyError);
00095     temp =  *test;
00096 
00097     if(temp == 0x7FC00000)
00098     {
00099         return(0);
00100     }
00101     
00102 
00103   SNR = 10 * log10 (EnergySignal / EnergyError);
00104 
00105   return (SNR);
00106 
00107 }
00108 
00109 
00110 /** 
00111  * @brief  Provide guard bits for Input buffer
00112  * @param  q15_t*       Pointer to input buffer
00113  * @param  uint32_t     blockSize
00114  * @param  uint32_t     guard_bits
00115  * @return none
00116  * The function Provides the guard bits for the buffer 
00117  * to avoid overflow 
00118  */
00119 
00120 void arm_provide_guard_bits_q15 (q15_t * input_buf, uint32_t blockSize,
00121                             uint32_t guard_bits)
00122 {
00123   uint32_t i;
00124 
00125   for (i = 0; i < blockSize; i++)
00126     {
00127       input_buf[i] = input_buf[i] >> guard_bits;
00128     }
00129 }
00130 
00131 /** 
00132  * @brief  Converts float to fixed in q12.20 format
00133  * @param  uint32_t     number of samples in the buffer
00134  * @return none
00135  * The function converts floating point values to fixed point(q12.20) values 
00136  */
00137 
00138 void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples)
00139 {
00140   uint32_t i;
00141 
00142   for (i = 0; i < numSamples; i++)
00143     {
00144       /* 1048576.0f corresponds to pow(2, 20) */
00145       pOut[i] = (q31_t) (pIn[i] * 1048576.0f);
00146 
00147       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
00148 
00149       if (pIn[i] == (float) 1.0)
00150         {
00151           pOut[i] = 0x000FFFFF;
00152         }
00153     }
00154 }
00155 
00156 /** 
00157  * @brief  Compare MATLAB Reference Output and ARM Test output
00158  * @param  q15_t*   Pointer to Ref buffer
00159  * @param  q15_t*   Pointer to Test buffer
00160  * @param  uint32_t     number of samples in the buffer
00161  * @return none 
00162  */
00163 
00164 uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t * pOut, uint32_t numSamples)
00165 {
00166   uint32_t i; 
00167   int32_t diff, diffCrnt = 0;
00168   uint32_t maxDiff = 0;
00169 
00170   for (i = 0; i < numSamples; i++)
00171   {
00172     diff = pIn[i] - pOut[i];
00173     diffCrnt = (diff > 0) ? diff : -diff;
00174 
00175     if(diffCrnt > maxDiff)
00176     {
00177         maxDiff = diffCrnt;
00178     }   
00179   }
00180 
00181   return(maxDiff);
00182 }
00183 
00184 /** 
00185  * @brief  Compare MATLAB Reference Output and ARM Test output
00186  * @param  q31_t*   Pointer to Ref buffer
00187  * @param  q31_t*   Pointer to Test buffer
00188  * @param  uint32_t     number of samples in the buffer
00189  * @return none 
00190  */
00191 
00192 uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t * pOut, uint32_t numSamples)
00193 {
00194   uint32_t i; 
00195   int32_t diff, diffCrnt = 0;
00196   uint32_t maxDiff = 0;
00197 
00198   for (i = 0; i < numSamples; i++)
00199   {
00200     diff = pIn[i] - pOut[i];
00201     diffCrnt = (diff > 0) ? diff : -diff;
00202 
00203     if(diffCrnt > maxDiff)
00204     {
00205         maxDiff = diffCrnt;
00206     }
00207   }
00208 
00209   return(maxDiff);
00210 }
00211 
00212 /** 
00213  * @brief  Provide guard bits for Input buffer
00214  * @param  q31_t*   Pointer to input buffer
00215  * @param  uint32_t     blockSize
00216  * @param  uint32_t     guard_bits
00217  * @return none
00218  * The function Provides the guard bits for the buffer 
00219  * to avoid overflow 
00220  */
00221 
00222 void arm_provide_guard_bits_q31 (q31_t * input_buf, 
00223                                  uint32_t blockSize,
00224                                  uint32_t guard_bits)
00225 {
00226   uint32_t i;
00227 
00228   for (i = 0; i < blockSize; i++)
00229     {
00230       input_buf[i] = input_buf[i] >> guard_bits;
00231     }
00232 }
00233 
00234 /** 
00235  * @brief  Provide guard bits for Input buffer
00236  * @param  q31_t*   Pointer to input buffer
00237  * @param  uint32_t     blockSize
00238  * @param  uint32_t     guard_bits
00239  * @return none
00240  * The function Provides the guard bits for the buffer 
00241  * to avoid overflow 
00242  */
00243 
00244 void arm_provide_guard_bits_q7 (q7_t * input_buf, 
00245                                 uint32_t blockSize,
00246                                 uint32_t guard_bits)
00247 {
00248   uint32_t i;
00249 
00250   for (i = 0; i < blockSize; i++)
00251     {
00252       input_buf[i] = input_buf[i] >> guard_bits;
00253     }
00254 }
00255 
00256 
00257 
00258 /** 
00259  * @brief  Caluclates number of guard bits 
00260  * @param  uint32_t     number of additions
00261  * @return none
00262  * The function Caluclates the number of guard bits  
00263  * depending on the numtaps 
00264  */
00265 
00266 uint32_t arm_calc_guard_bits (uint32_t num_adds)
00267 {
00268   uint32_t i = 1, j = 0;
00269 
00270   if (num_adds == 1)
00271     {
00272       return (0);
00273     }
00274 
00275   while (i < num_adds)
00276     {
00277       i = i * 2;
00278       j++;
00279     }
00280 
00281   return (j);
00282 }
00283 
00284 /** 
00285  * @brief  Converts Q15 to floating-point
00286  * @param  uint32_t     number of samples in the buffer
00287  * @return none
00288  */
00289 
00290 void arm_apply_guard_bits (float32_t * pIn, 
00291                            uint32_t numSamples, 
00292                            uint32_t guard_bits)
00293 {
00294   uint32_t i;
00295 
00296   for (i = 0; i < numSamples; i++)
00297     {
00298       pIn[i] = pIn[i] * arm_calc_2pow(guard_bits);
00299     }
00300 }
00301 
00302 /** 
00303  * @brief  Calculates pow(2, numShifts)
00304  * @param  uint32_t     number of shifts
00305  * @return pow(2, numShifts)
00306  */
00307 uint32_t arm_calc_2pow(uint32_t numShifts)
00308 {
00309 
00310   uint32_t i, val = 1;
00311 
00312   for (i = 0; i < numShifts; i++)
00313     {
00314       val = val * 2;
00315     }   
00316 
00317   return(val);
00318 }
00319 
00320 
00321 
00322 /** 
00323  * @brief  Converts float to fixed q14 
00324  * @param  uint32_t     number of samples in the buffer
00325  * @return none
00326  * The function converts floating point values to fixed point values 
00327  */
00328 
00329 void arm_float_to_q14 (float *pIn, q15_t * pOut, 
00330                        uint32_t numSamples)
00331 {
00332   uint32_t i;
00333 
00334   for (i = 0; i < numSamples; i++)
00335     {
00336       /* 16384.0f corresponds to pow(2, 14) */
00337       pOut[i] = (q15_t) (pIn[i] * 16384.0f);
00338 
00339       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
00340 
00341       if (pIn[i] == (float) 2.0)
00342         {
00343           pOut[i] = 0x7FFF;
00344         }
00345 
00346     }
00347 
00348 }
00349 
00350  
00351 /** 
00352  * @brief  Converts float to fixed q30 format
00353  * @param  uint32_t     number of samples in the buffer
00354  * @return none
00355  * The function converts floating point values to fixed point values 
00356  */
00357 
00358 void arm_float_to_q30 (float *pIn, q31_t * pOut, 
00359                        uint32_t numSamples)
00360 {
00361   uint32_t i;
00362 
00363   for (i = 0; i < numSamples; i++)
00364     {
00365       /* 1073741824.0f corresponds to pow(2, 30) */
00366       pOut[i] = (q31_t) (pIn[i] * 1073741824.0f);
00367 
00368       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
00369 
00370       if (pIn[i] == (float) 2.0)
00371         {
00372           pOut[i] = 0x7FFFFFFF;
00373         }
00374     }
00375 }
00376 
00377 /** 
00378  * @brief  Converts float to fixed q30 format
00379  * @param  uint32_t     number of samples in the buffer
00380  * @return none
00381  * The function converts floating point values to fixed point values 
00382  */
00383 
00384 void arm_float_to_q29 (float *pIn, q31_t * pOut, 
00385                        uint32_t numSamples)
00386 {
00387   uint32_t i;
00388 
00389   for (i = 0; i < numSamples; i++)
00390     {
00391       /* 1073741824.0f corresponds to pow(2, 30) */
00392       pOut[i] = (q31_t) (pIn[i] * 536870912.0f);
00393 
00394       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
00395 
00396       if (pIn[i] == (float) 4.0)
00397         {
00398           pOut[i] = 0x7FFFFFFF;
00399         }
00400     }
00401 }
00402 
00403 
00404 /** 
00405  * @brief  Converts float to fixed q28 format
00406  * @param  uint32_t     number of samples in the buffer
00407  * @return none
00408  * The function converts floating point values to fixed point values 
00409  */
00410 
00411 void arm_float_to_q28 (float *pIn, q31_t * pOut, 
00412                        uint32_t numSamples)
00413 {
00414   uint32_t i;
00415 
00416   for (i = 0; i < numSamples; i++)
00417     {
00418     /* 268435456.0f corresponds to pow(2, 28) */
00419       pOut[i] = (q31_t) (pIn[i] * 268435456.0f);
00420 
00421       pOut[i] += pIn[i] > 0 ? 0.5 : -0.5;
00422 
00423       if (pIn[i] == (float) 8.0)
00424         {
00425           pOut[i] = 0x7FFFFFFF;
00426         }
00427     }
00428 }
00429 
00430 /** 
00431  * @brief  Clip the float values to +/- 1 
00432  * @param  pIn  input buffer
00433  * @param  numSamples   number of samples in the buffer
00434  * @return none
00435  * The function converts floating point values to fixed point values 
00436  */
00437 
00438 void arm_clip_f32 (float *pIn, uint32_t numSamples)
00439 {
00440   uint32_t i;
00441 
00442   for (i = 0; i < numSamples; i++)
00443     {
00444       if(pIn[i] > 1.0f)
00445       {
00446         pIn[i] = 1.0;
00447       }
00448       else if( pIn[i] < -1.0f)
00449       {
00450         pIn[i] = -1.0;
00451       }
00452            
00453     }
00454 }
00455 
00456 
00457 
00458