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 b 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[in] pRef Pointer to the reference buffer 55 * @param[in] pTest Pointer to the test buffer 56 * @param[in] buffSize total number of samples 57 * @return 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[in,out] input_buf Pointer to input buffer 113 * @param[in] blockSize block Size 114 * @param[in] guard_bits 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[in] pIn pointer to input buffer 134 * @param[out] pOut pointer to outputbuffer 135 * @param[in] numSamples number of samples in the input buffer 136 * @return none 137 * The function converts floating point values to fixed point(q12.20) values 138 */ 139 140 void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples) 141 { 142 uint32_t i; 143 144 for (i = 0; i < numSamples; i++) 145 { 146 /* 1048576.0f corresponds to pow(2, 20) */ 147 pOut[i] = (q31_t) (pIn[i] * 1048576.0f); 148 149 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; 150 151 if (pIn[i] == (float) 1.0) 152 { 153 pOut[i] = 0x000FFFFF; 154 } 155 } 156 } 157 158 /** 159 * @brief Compare MATLAB Reference Output and ARM Test output 160 * @param[in] pIn Pointer to Ref buffer 161 * @param[in] pOut Pointer to Test buffer 162 * @param[in] numSamples number of samples in the buffer 163 * @return maximum difference 164 */ 165 166 uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t *pOut, uint32_t numSamples) 167 { 168 uint32_t i; 169 int32_t diff, diffCrnt = 0; 170 uint32_t maxDiff = 0; 171 172 for (i = 0; i < numSamples; i++) 173 { 174 diff = pIn[i] - pOut[i]; 175 diffCrnt = (diff > 0) ? diff : -diff; 176 177 if (diffCrnt > maxDiff) 178 { 179 maxDiff = diffCrnt; 180 } 181 } 182 183 return(maxDiff); 184 } 185 186 /** 187 * @brief Compare MATLAB Reference Output and ARM Test output 188 * @param[in] pIn Pointer to Ref buffer 189 * @param[in] pOut Pointer to Test buffer 190 * @param[in] numSamples number of samples in the buffer 191 * @return maximum difference 192 */ 193 194 uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t * pOut, uint32_t numSamples) 195 { 196 uint32_t i; 197 int32_t diff, 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[in,out] input_buf Pointer to input buffer 217 * @param[in] blockSize block Size 218 * @param[in] guard_bits 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[in,out] input_buf Pointer to input buffer 239 * @param[in] blockSize block Size 240 * @param[in] guard_bits 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[in] num_adds number of additions 263 * @return guard bits 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 Apply guard bits to buffer 288 * @param[in,out] pIn pointer to input buffer 289 * @param[in] numSamples number of samples in the input buffer 290 * @param[in] guard_bits guard bits 291 * @return none 292 */ 293 294 void arm_apply_guard_bits (float32_t *pIn, 295 uint32_t numSamples, 296 uint32_t guard_bits) 297 { 298 uint32_t i; 299 300 for (i = 0; i < numSamples; i++) 301 { 302 pIn[i] = pIn[i] * arm_calc_2pow(guard_bits); 303 } 304 } 305 306 /** 307 * @brief Calculates pow(2, numShifts) 308 * @param[in] numShifts number of shifts 309 * @return pow(2, numShifts) 310 */ 311 uint32_t arm_calc_2pow(uint32_t numShifts) 312 { 313 314 uint32_t i, val = 1; 315 316 for (i = 0; i < numShifts; i++) 317 { 318 val = val * 2; 319 } 320 321 return(val); 322 } 323 324 325 326 /** 327 * @brief Converts float to fixed q14 328 * @param[in] pIn pointer to input buffer 329 * @param[out] pOut pointer to output buffer 330 * @param[in] numSamples number of samples in the buffer 331 * @return none 332 * The function converts floating point values to fixed point values 333 */ 334 335 void arm_float_to_q14 (float *pIn, q15_t *pOut, uint32_t numSamples) 336 { 337 uint32_t i; 338 339 for (i = 0; i < numSamples; i++) 340 { 341 /* 16384.0f corresponds to pow(2, 14) */ 342 pOut[i] = (q15_t) (pIn[i] * 16384.0f); 343 344 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; 345 346 if (pIn[i] == (float) 2.0) 347 { 348 pOut[i] = 0x7FFF; 349 } 350 351 } 352 353 } 354 355 356 /** 357 * @brief Converts float to fixed q30 format 358 * @param[in] pIn pointer to input buffer 359 * @param[out] pOut pointer to output buffer 360 * @param[in] numSamples number of samples in the buffer 361 * @return none 362 * The function converts floating point values to fixed point values 363 */ 364 365 void arm_float_to_q30 (float *pIn, q31_t * pOut, uint32_t numSamples) 366 { 367 uint32_t i; 368 369 for (i = 0; i < numSamples; i++) 370 { 371 /* 1073741824.0f corresponds to pow(2, 30) */ 372 pOut[i] = (q31_t) (pIn[i] * 1073741824.0f); 373 374 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; 375 376 if (pIn[i] == (float) 2.0) 377 { 378 pOut[i] = 0x7FFFFFFF; 379 } 380 } 381 } 382 383 /** 384 * @brief Converts float to fixed q30 format 385 * @param[in] pIn pointer to input buffer 386 * @param[out] pOut pointer to output buffer 387 * @param[in] numSamples number of samples in the buffer 388 * @return none 389 * The function converts floating point values to fixed point values 390 */ 391 392 void arm_float_to_q29 (float *pIn, q31_t *pOut, uint32_t numSamples) 393 { 394 uint32_t i; 395 396 for (i = 0; i < numSamples; i++) 397 { 398 /* 1073741824.0f corresponds to pow(2, 30) */ 399 pOut[i] = (q31_t) (pIn[i] * 536870912.0f); 400 401 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; 402 403 if (pIn[i] == (float) 4.0) 404 { 405 pOut[i] = 0x7FFFFFFF; 406 } 407 } 408 } 409 410 411 /** 412 * @brief Converts float to fixed q28 format 413 * @param[in] pIn pointer to input buffer 414 * @param[out] pOut pointer to output buffer 415 * @param[in] numSamples number of samples in the buffer 416 * @return none 417 * The function converts floating point values to fixed point values 418 */ 419 420 void arm_float_to_q28 (float *pIn, q31_t *pOut, uint32_t numSamples) 421 { 422 uint32_t i; 423 424 for (i = 0; i < numSamples; i++) 425 { 426 /* 268435456.0f corresponds to pow(2, 28) */ 427 pOut[i] = (q31_t) (pIn[i] * 268435456.0f); 428 429 pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; 430 431 if (pIn[i] == (float) 8.0) 432 { 433 pOut[i] = 0x7FFFFFFF; 434 } 435 } 436 } 437 438 439 /* 440 441 Conflicting with the new clip functions in CMSIS-DSP and not used 442 in the examples. 443 444 */ 445 #if 0 446 /** 447 * @brief Clip the float values to +/- 1 448 * @param[in,out] pIn input buffer 449 * @param[in] numSamples number of samples in the buffer 450 * @return none 451 * The function converts floating point values to fixed point values 452 */ 453 454 void arm_clip_f32 (float *pIn, uint32_t numSamples) 455 { 456 uint32_t i; 457 458 for (i = 0; i < numSamples; i++) 459 { 460 if (pIn[i] > 1.0f) 461 { 462 pIn[i] = 1.0; 463 } 464 else if ( pIn[i] < -1.0f) 465 { 466 pIn[i] = -1.0; 467 } 468 469 } 470 } 471 472 #endif