tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/SupportFunctions/math_helper.c

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