CMSIS DSP Library from CMSIS 2.0. See http://www.onarm.com/cmsis/ for full details
Dependents: K22F_DSP_Matrix_least_square BNO055-ELEC3810 1BNO055 ECE4180Project--Slave2 ... more
Diff: src/Cortex-M4-M3/FastMathFunctions/arm_sqrt_q15.c
- Revision:
- 0:1014af42efd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Cortex-M4-M3/FastMathFunctions/arm_sqrt_q15.c Thu Mar 10 15:07:50 2011 +0000 @@ -0,0 +1,123 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010 ARM Limited. All rights reserved. +* +* $Date: 29. November 2010 +* $Revision: V1.0.3 +* +* Project: CMSIS DSP Library +* Title: arm_sqrt_q15.c +* +* Description: Q15 square root function. +* +* 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. +* -------------------------------------------------------------------- */ + +#include "arm_math.h" +#include "arm_common_tables.h" + + +/** + * @ingroup groupFastMath + */ + +/** + * @addtogroup SQRT + * @{ + */ + + /** + * @brief Q15 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF. + * @param[out] *pOut square root of input value. + * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if + * <code>in</code> is negative value and returns zero output for negative values. + */ + +arm_status arm_sqrt_q15( + q15_t in, + q15_t * pOut) +{ + q31_t out; + q31_t prevOut; + q15_t oneByOut; + uint32_t sign_bits; + + if(in > 0) + { + /* run for ten iterations */ + + /* Take initial guess as half of the input and first iteration */ + out = ((q31_t) in >> 1u) + 0x3FFF; + + /* Calculation of reciprocal of out */ + /* oneByOut contains reciprocal of out which is in 2.14 format + and oneByOut should be upscaled by signBits */ + sign_bits = arm_recip_q15((q15_t) out, &oneByOut, (q15_t*)armRecipTableQ15); + + /* 0.5 * (out) */ + out = out >> 1u; + /* prevOut = 0.5 * out + (in * (oneByOut << signBits))) */ + prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + /* Third iteration */ + sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, (q15_t*)armRecipTableQ15); + prevOut = prevOut >> 1u; + out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + sign_bits = arm_recip_q15((q15_t) out, &oneByOut, (q15_t*)armRecipTableQ15); + out = out >> 1u; + prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + /* Fifth iteration */ + sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, (q15_t*)armRecipTableQ15); + prevOut = prevOut >> 1u; + out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + sign_bits = arm_recip_q15((q15_t) out, &oneByOut, (q15_t*)armRecipTableQ15); + out = out >> 1u; + prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + /* Seventh iteration */ + sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, (q15_t*)armRecipTableQ15); + prevOut = prevOut >> 1u; + out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + sign_bits = arm_recip_q15((q15_t) out, &oneByOut, (q15_t*)armRecipTableQ15); + out = out >> 1u; + prevOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + sign_bits = arm_recip_q15((q15_t) prevOut, &oneByOut, (q15_t*)armRecipTableQ15); + prevOut = prevOut >> 1u; + out = prevOut + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + /* tenth iteration */ + sign_bits = arm_recip_q15((q15_t) out, &oneByOut, (q15_t*)armRecipTableQ15); + out = out >> 1u; + *pOut = out + (((q15_t) (((q31_t) in * oneByOut) >> 16)) << sign_bits); + + return (ARM_MATH_SUCCESS); + } + else + { + + *pOut = 0; + return (ARM_MATH_ARGUMENT_ERROR); + } + +} + +/** + * @} end of SQRT group + */