math_helper.c
419 lines
| 9.1 KiB
| text/x-c
|
CLexer
|
r16 | /* ---------------------------------------------------------------------- | ||
* Copyright (C) 2010 ARM Limited. All rights reserved. | ||||
* | ||||
* $Date: 29. November 2010 | ||||
* $Revision: V1.0.3 | ||||
* | ||||
* Project: CMSIS DSP Library | ||||
* | ||||
* Title: math_helper.c | ||||
* | ||||
* Description: Definition of all helper functions required. | ||||
* | ||||
* Target Processor: Cortex-M4/Cortex-M3 | ||||
* | ||||
* Version 1.0.3 2010/11/29 | ||||
* Re-organized the CMSIS folders and updated documentation. | ||||
* | ||||
* Version 1.0.2 2010/11/11 | ||||
* Documentation updated. | ||||
* | ||||
* Version 1.0.1 2010/10/05 | ||||
* Production release and review comments incorporated. | ||||
* | ||||
* Version 1.0.0 2010/09/20 | ||||
* Production release and review comments incorporated. | ||||
* | ||||
* Version 0.0.7 2010/06/10 | ||||
* Misra-C changes done | ||||
* -------------------------------------------------------------------- */ | ||||
/* ---------------------------------------------------------------------- | ||||
* Include standard header files | ||||
* -------------------------------------------------------------------- */ | ||||
#include<math.h> | ||||
/* ---------------------------------------------------------------------- | ||||
* Include project header files | ||||
* -------------------------------------------------------------------- */ | ||||
#include "math_helper.h" | ||||
/** | ||||
* @brief Caluclation of SNR | ||||
* @param float* Pointer to the reference buffer | ||||
* @param float* Pointer to the test buffer | ||||
* @param uint32_t total number of samples | ||||
* @return float SNR | ||||
* The function Caluclates signal to noise ratio for the reference output | ||||
* and test output | ||||
*/ | ||||
float arm_snr_f32(float *pRef, float *pTest, uint32_t buffSize) | ||||
{ | ||||
float EnergySignal = 0.0, EnergyError = 0.0; | ||||
uint32_t i; | ||||
float SNR; | ||||
for (i = 0; i < buffSize; i++) | ||||
{ | ||||
EnergySignal += pRef[i] * pRef[i]; | ||||
EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]); | ||||
} | ||||
SNR = 10 * log10 (EnergySignal / EnergyError); | ||||
return (SNR); | ||||
} | ||||
/** | ||||
* @brief Provide guard bits for Input buffer | ||||
* @param q15_t* Pointer to input buffer | ||||
* @param uint32_t blockSize | ||||
* @param uint32_t guard_bits | ||||
* @return none | ||||
* The function Provides the guard bits for the buffer | ||||
* to avoid overflow | ||||
*/ | ||||
void arm_provide_guard_bits_q15 (q15_t * input_buf, uint32_t blockSize, | ||||
uint32_t guard_bits) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < blockSize; i++) | ||||
{ | ||||
input_buf[i] = input_buf[i] >> guard_bits; | ||||
} | ||||
} | ||||
/** | ||||
* @brief Converts float to fixed in q12.20 format | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point(q12.20) values | ||||
*/ | ||||
void arm_float_to_q12_20(float *pIn, q31_t * pOut, uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
/* 1048576.0f corresponds to pow(2, 20) */ | ||||
pOut[i] = (q31_t) (pIn[i] * 1048576.0f); | ||||
pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; | ||||
if (pIn[i] == (float) 1.0) | ||||
{ | ||||
pOut[i] = 0x000FFFFF; | ||||
} | ||||
} | ||||
} | ||||
/** | ||||
* @brief Compare MATLAB Reference Output and ARM Test output | ||||
* @param q15_t* Pointer to Ref buffer | ||||
* @param q15_t* Pointer to Test buffer | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
*/ | ||||
uint32_t arm_compare_fixed_q15(q15_t *pIn, q15_t * pOut, uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
int32_t diff, diffCrnt = 0; | ||||
uint32_t maxDiff = 0; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
diff = pIn[i] - pOut[i]; | ||||
diffCrnt = (diff > 0) ? diff : -diff; | ||||
if(diffCrnt > maxDiff) | ||||
{ | ||||
maxDiff = diffCrnt; | ||||
} | ||||
} | ||||
return(maxDiff); | ||||
} | ||||
/** | ||||
* @brief Compare MATLAB Reference Output and ARM Test output | ||||
* @param q31_t* Pointer to Ref buffer | ||||
* @param q31_t* Pointer to Test buffer | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
*/ | ||||
uint32_t arm_compare_fixed_q31(q31_t *pIn, q31_t * pOut, uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
int32_t diff, diffCrnt = 0; | ||||
uint32_t maxDiff = 0; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
diff = pIn[i] - pOut[i]; | ||||
diffCrnt = (diff > 0) ? diff : -diff; | ||||
if(diffCrnt > maxDiff) | ||||
{ | ||||
maxDiff = diffCrnt; | ||||
} | ||||
} | ||||
return(maxDiff); | ||||
} | ||||
/** | ||||
* @brief Provide guard bits for Input buffer | ||||
* @param q31_t* Pointer to input buffer | ||||
* @param uint32_t blockSize | ||||
* @param uint32_t guard_bits | ||||
* @return none | ||||
* The function Provides the guard bits for the buffer | ||||
* to avoid overflow | ||||
*/ | ||||
void arm_provide_guard_bits_q31 (q31_t * input_buf, | ||||
uint32_t blockSize, | ||||
uint32_t guard_bits) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < blockSize; i++) | ||||
{ | ||||
input_buf[i] = input_buf[i] >> guard_bits; | ||||
} | ||||
} | ||||
/** | ||||
* @brief Provide guard bits for Input buffer | ||||
* @param q31_t* Pointer to input buffer | ||||
* @param uint32_t blockSize | ||||
* @param uint32_t guard_bits | ||||
* @return none | ||||
* The function Provides the guard bits for the buffer | ||||
* to avoid overflow | ||||
*/ | ||||
void arm_provide_guard_bits_q7 (q7_t * input_buf, | ||||
uint32_t blockSize, | ||||
uint32_t guard_bits) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < blockSize; i++) | ||||
{ | ||||
input_buf[i] = input_buf[i] >> guard_bits; | ||||
} | ||||
} | ||||
/** | ||||
* @brief Caluclates number of guard bits | ||||
* @param uint32_t number of additions | ||||
* @return none | ||||
* The function Caluclates the number of guard bits | ||||
* depending on the numtaps | ||||
*/ | ||||
uint32_t arm_calc_guard_bits (uint32_t num_adds) | ||||
{ | ||||
uint32_t i = 1, j = 0; | ||||
if (num_adds == 1) | ||||
{ | ||||
return (0); | ||||
} | ||||
while (i < num_adds) | ||||
{ | ||||
i = i * 2; | ||||
j++; | ||||
} | ||||
return (j); | ||||
} | ||||
/** | ||||
* @brief Converts Q15 to floating-point | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
*/ | ||||
void arm_apply_guard_bits (float32_t * pIn, | ||||
uint32_t numSamples, | ||||
uint32_t guard_bits) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
pIn[i] = pIn[i] * arm_calc_2pow(guard_bits); | ||||
} | ||||
} | ||||
/** | ||||
* @brief Calculates pow(2, numShifts) | ||||
* @param uint32_t number of shifts | ||||
* @return pow(2, numShifts) | ||||
*/ | ||||
uint32_t arm_calc_2pow(uint32_t numShifts) | ||||
{ | ||||
uint32_t i, val = 1; | ||||
for (i = 0; i < numShifts; i++) | ||||
{ | ||||
val = val * 2; | ||||
} | ||||
return(val); | ||||
} | ||||
/** | ||||
* @brief Converts float to fixed q14 | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point values | ||||
*/ | ||||
void arm_float_to_q14 (float *pIn, q15_t * pOut, | ||||
uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
/* 16384.0f corresponds to pow(2, 14) */ | ||||
pOut[i] = (q15_t) (pIn[i] * 16384.0f); | ||||
pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; | ||||
if (pIn[i] == (float) 2.0) | ||||
{ | ||||
pOut[i] = 0x7FFF; | ||||
} | ||||
} | ||||
} | ||||
/** | ||||
* @brief Converts float to fixed q30 format | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point values | ||||
*/ | ||||
void arm_float_to_q30 (float *pIn, q31_t * pOut, | ||||
uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
/* 1073741824.0f corresponds to pow(2, 30) */ | ||||
pOut[i] = (q31_t) (pIn[i] * 1073741824.0f); | ||||
pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; | ||||
if (pIn[i] == (float) 2.0) | ||||
{ | ||||
pOut[i] = 0x7FFFFFFF; | ||||
} | ||||
} | ||||
} | ||||
/** | ||||
* @brief Converts float to fixed q30 format | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point values | ||||
*/ | ||||
void arm_float_to_q29 (float *pIn, q31_t * pOut, | ||||
uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
/* 1073741824.0f corresponds to pow(2, 30) */ | ||||
pOut[i] = (q31_t) (pIn[i] * 536870912.0f); | ||||
pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; | ||||
if (pIn[i] == (float) 4.0) | ||||
{ | ||||
pOut[i] = 0x7FFFFFFF; | ||||
} | ||||
} | ||||
} | ||||
/** | ||||
* @brief Converts float to fixed q28 format | ||||
* @param uint32_t number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point values | ||||
*/ | ||||
void arm_float_to_q28 (float *pIn, q31_t * pOut, | ||||
uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
/* 268435456.0f corresponds to pow(2, 28) */ | ||||
pOut[i] = (q31_t) (pIn[i] * 268435456.0f); | ||||
pOut[i] += pIn[i] > 0 ? 0.5 : -0.5; | ||||
if (pIn[i] == (float) 8.0) | ||||
{ | ||||
pOut[i] = 0x7FFFFFFF; | ||||
} | ||||
} | ||||
} | ||||
/** | ||||
* @brief Clip the float values to +/- 1 | ||||
* @param pIn input buffer | ||||
* @param numSamples number of samples in the buffer | ||||
* @return none | ||||
* The function converts floating point values to fixed point values | ||||
*/ | ||||
void arm_clip_f32 (float *pIn, uint32_t numSamples) | ||||
{ | ||||
uint32_t i; | ||||
for (i = 0; i < numSamples; i++) | ||||
{ | ||||
if(pIn[i] > 1.0f) | ||||
{ | ||||
pIn[i] = 1.0; | ||||
} | ||||
else if( pIn[i] < -1.0f) | ||||
{ | ||||
pIn[i] = -1.0; | ||||
} | ||||
} | ||||
} | ||||