]>
git.gir.st - tmk_keyboard.git/blob - tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_sqrt_q31.c
1 /* ----------------------------------------------------------------------
2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
4 * $Date: 17. January 2013
7 * Project: CMSIS DSP Library
8 * Title: arm_sqrt_q31.c
10 * Description: Q31 square root function.
12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
14 * Redistribution and use in source and binary forms, with or without
15 * modification, are permitted provided that the following conditions
17 * - Redistributions of source code must retain the above copyright
18 * notice, this list of conditions and the following disclaimer.
19 * - Redistributions in binary form must reproduce the above copyright
20 * notice, this list of conditions and the following disclaimer in
21 * the documentation and/or other materials provided with the
23 * - Neither the name of ARM LIMITED nor the names of its contributors
24 * may be used to endorse or promote products derived from this
25 * software without specific prior written permission.
27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
38 * POSSIBILITY OF SUCH DAMAGE.
39 * -------------------------------------------------------------------- */
41 #include "arm_common_tables.h"
44 * @ingroup groupFastMath
53 * @brief Q31 square root function.
54 * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
55 * @param[out] *pOut square root of input value.
56 * @return The function returns ARM_MATH_SUCCESS if the input value is positive
57 * and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
58 * negative inputs, the function returns *pOut = 0.
61 arm_status
arm_sqrt_q31(
65 q31_t number
, temp1
, bits_val1
, var1
, signBits1
, half
;
66 float32_t temp_float1
;
75 /* If the input is a positive number then compute the signBits. */
78 signBits1
= __CLZ(number
) - 1;
80 /* Shift by the number of signBits1 */
81 if((signBits1
% 2) == 0)
83 number
= number
<< signBits1
;
87 number
= number
<< (signBits1
- 1);
90 /* Calculate half value of the number */
92 /* Store the number for later use */
96 temp_float1
= number
* 4.6566128731e-010f
;
98 tempconv
.floatval
= temp_float1
;
99 bits_val1
= tempconv
.fracval
;
100 /* Subtract the shifted value from the magic number to give intial guess */
101 bits_val1
= 0x5f3759df - (bits_val1
>> 1); // gives initial guess
103 tempconv
.fracval
= bits_val1
;
104 temp_float1
= tempconv
.floatval
;
105 /* Convert to integer format */
106 var1
= (q31_t
) (temp_float1
* 1073741824);
109 var1
= ((q31_t
) ((q63_t
) var1
* (0x30000000 -
112 (((q63_t
) var1
* var1
) >> 31)) *
113 (q63_t
) half
) >> 31))) >> 31)) << 2;
115 var1
= ((q31_t
) ((q63_t
) var1
* (0x30000000 -
118 (((q63_t
) var1
* var1
) >> 31)) *
119 (q63_t
) half
) >> 31))) >> 31)) << 2;
121 var1
= ((q31_t
) ((q63_t
) var1
* (0x30000000 -
124 (((q63_t
) var1
* var1
) >> 31)) *
125 (q63_t
) half
) >> 31))) >> 31)) << 2;
127 /* Multiply the inverse square root with the original value */
128 var1
= ((q31_t
) (((q63_t
) temp1
* var1
) >> 31)) << 1;
130 /* Shift the output down accordingly */
131 if((signBits1
% 2) == 0)
133 var1
= var1
>> (signBits1
/ 2);
137 var1
= var1
>> ((signBits1
- 1) / 2);
141 return (ARM_MATH_SUCCESS
);
143 /* If the number is a negative number then store zero as its square root value */
147 return (ARM_MATH_ARGUMENT_ERROR
);
152 * @} end of SQRT group