| /* ---------------------------------------------------------------------- |
| * Copyright (C) 2010-2012 ARM Limited. All rights reserved. |
| * |
| * $Date: 17. January 2013 |
| * $Revision: V1.4.0 b |
| * |
| * Project: CMSIS DSP Library |
| * |
| * Title: math_helper.c |
| * |
| * Description: Definition of all helper functions required. |
| * |
| * Target Processor: Cortex-M4/Cortex-M3 |
| * |
| * Redistribution and use in source and binary forms, with or without |
| * modification, are permitted provided that the following conditions |
| * are met: |
| * - Redistributions of source code must retain the above copyright |
| * notice, this list of conditions and the following disclaimer. |
| * - Redistributions in binary form must reproduce the above copyright |
| * notice, this list of conditions and the following disclaimer in |
| * the documentation and/or other materials provided with the |
| * distribution. |
| * - Neither the name of ARM LIMITED nor the names of its contributors |
| * may be used to endorse or promote products derived from this |
| * software without specific prior written permission. |
| * |
| * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
| * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
| * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
| * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
| * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
| * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
| * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
| * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
| * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
| * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
| * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| * POSSIBILITY OF SUCH DAMAGE. |
| * -------------------------------------------------------------------- */ |
| |
| /* ---------------------------------------------------------------------- |
| * Include standard header files |
| * -------------------------------------------------------------------- */ |
| #include<math.h> |
| |
| /* ---------------------------------------------------------------------- |
| * Include project header files |
| * -------------------------------------------------------------------- */ |
| #include "math_helper.h" |
| |
| /** |
| * @brief Caluclation of SNR |
| * @param[in] pRef Pointer to the reference buffer |
| * @param[in] pTest Pointer to the test buffer |
| * @param[in] buffSize total number of samples |
| * @return 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[in,out] input_buf Pointer to input buffer |
| * @param[in] blockSize block Size |
| * @param[in] guard_bits 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[in] pIn pointer to input buffer |
| * @param[out] pOut pointer to outputbuffer |
| * @param[in] numSamples number of samples in the input 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[in] pIn Pointer to Ref buffer |
| * @param[in] pOut Pointer to Test buffer |
| * @param[in] numSamples number of samples in the buffer |
| * @return maximum difference |
| */ |
| |
| 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[in] pIn Pointer to Ref buffer |
| * @param[in] pOut Pointer to Test buffer |
| * @param[in] numSamples number of samples in the buffer |
| * @return maximum difference |
| */ |
| |
| 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[in,out] input_buf Pointer to input buffer |
| * @param[in] blockSize block Size |
| * @param[in] guard_bits 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[in,out] input_buf Pointer to input buffer |
| * @param[in] blockSize block Size |
| * @param[in] guard_bits 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[in] num_adds number of additions |
| * @return guard bits |
| * 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 Apply guard bits to buffer |
| * @param[in,out] pIn pointer to input buffer |
| * @param[in] numSamples number of samples in the input buffer |
| * @param[in] guard_bits guard bits |
| * @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[in] numShifts 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[in] pIn pointer to input buffer |
| * @param[out] pOut pointer to output buffer |
| * @param[in] numSamples 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[in] pIn pointer to input buffer |
| * @param[out] pOut pointer to output buffer |
| * @param[in] numSamples 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[in] pIn pointer to input buffer |
| * @param[out] pOut pointer to output buffer |
| * @param[in] numSamples 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[in] pIn pointer to input buffer |
| * @param[out] pOut pointer to output buffer |
| * @param[in] numSamples 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[in,out] pIn input buffer |
| * @param[in] 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; |
| } |
| |
| } |
| } |
| |
| |
| |
| |
