mirror of https://github.com/ARMmbed/mbed-os.git
448 lines
9.2 KiB
C
448 lines
9.2 KiB
C
/* ----------------------------------------------------------------------
|
|
* 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;
|
|
int temp;
|
|
int *test;
|
|
|
|
for (i = 0; i < buffSize; i++)
|
|
{
|
|
/* Checking for a NAN value in pRef array */
|
|
test = (int *)(&pRef[i]);
|
|
temp = *test;
|
|
|
|
if(temp == 0x7FC00000)
|
|
{
|
|
return(0);
|
|
}
|
|
|
|
/* Checking for a NAN value in pTest array */
|
|
test = (int *)(&pTest[i]);
|
|
temp = *test;
|
|
|
|
if(temp == 0x7FC00000)
|
|
{
|
|
return(0);
|
|
}
|
|
EnergySignal += pRef[i] * pRef[i];
|
|
EnergyError += (pRef[i] - pTest[i]) * (pRef[i] - pTest[i]);
|
|
}
|
|
|
|
/* Checking for a NAN value in EnergyError */
|
|
test = (int *)(&EnergyError);
|
|
temp = *test;
|
|
|
|
if(temp == 0x7FC00000)
|
|
{
|
|
return(0);
|
|
}
|
|
|
|
|
|
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;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|