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_q31.c
- Revision:
- 0:1014af42efd9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/Cortex-M4-M3/FastMathFunctions/arm_sqrt_q31.c Thu Mar 10 15:07:50 2011 +0000 @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------- +* Copyright (C) 2010 ARM Limited. All rights reserved. +* +* $Date: 29. November 2010 +* $Revision: V1.0.3 +* +* Project: CMSIS DSP Library +* Title: arm_sqrt_q31.c +* +* Description: Q31 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 Q31 square root function. + * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF. + * @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_q31( + q31_t in, + q31_t * pOut) +{ + q63_t out; + q63_t prevOut; + q31_t oneByOut; + uint32_t signBits; + + + if(in > 0) + { + + /* run for ten iterations */ + + /* Take initial guess as half of the input and first iteration */ + out = (in >> 1) + 0x3FFFFFFF; + + /* Calculation of reciprocal of out */ + /* oneByOut contains reciprocal of out which is in 2.30 format + and oneByOut should be upscaled by signBits */ + signBits = arm_recip_q31((q31_t) out, &oneByOut, (q31_t*)armRecipTableQ31); + + /* 0.5 * (out) */ + out = out >> 1u; + + /* prevOut = 0.5 * out + (in * (oneByOut << signBits))) */ + prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + /* Third iteration */ + signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, (q31_t*)armRecipTableQ31); + prevOut = prevOut >> 1u; + out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + signBits = arm_recip_q31((q31_t) out, &oneByOut, (q31_t*)armRecipTableQ31); + out = out >> 1u; + prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + /* Fifth iteration */ + signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, (q31_t*)armRecipTableQ31); + prevOut = prevOut >> 1u; + out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + signBits = arm_recip_q31((q31_t) out, &oneByOut, (q31_t*)armRecipTableQ31); + out = out >> 1u; + prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + /* Seventh iteration */ + signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, (q31_t*)armRecipTableQ31); + prevOut = prevOut >> 1u; + out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + signBits = arm_recip_q31((q31_t) out, &oneByOut, (q31_t*)armRecipTableQ31); + out = out >> 1u; + prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, (q31_t*)armRecipTableQ31); + prevOut = prevOut >> 1u; + out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + /* tenth iteration */ + signBits = arm_recip_q31((q31_t) out, &oneByOut, (q31_t*)armRecipTableQ31); + out = out >> 1u; + *pOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits); + + return (ARM_MATH_SUCCESS); + } + else + { + *pOut = 0; + return (ARM_MATH_ARGUMENT_ERROR); + } + +} + +/** + * @} end of SQRT group + */