1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
|
/* ----------------------------------------------------------------------
* Copyright (C) 2010-2014 ARM Limited. All rights reserved.
*
* $Date: 12. March 2014
* $Revision: V1.4.4
*
* Project: CMSIS DSP Library
* Title: arm_sqrt_q15.c
*
* Description: Q15 square root function.
*
* Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
*
* 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 "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 the input value is positive
* and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
* negative inputs, the function returns *pOut = 0.
*/
arm_status arm_sqrt_q15(
q15_t in,
q15_t * pOut)
{
q15_t number, temp1, var1, signBits1, half;
q31_t bits_val1;
float32_t temp_float1;
union
{
q31_t fracval;
float32_t floatval;
} tempconv;
number = in;
/* If the input is a positive number then compute the signBits. */
if(number > 0)
{
signBits1 = __CLZ(number) - 17;
/* Shift by the number of signBits1 */
if((signBits1 % 2) == 0)
{
number = number << signBits1;
}
else
{
number = number << (signBits1 - 1);
}
/* Calculate half value of the number */
half = number >> 1;
/* Store the number for later use */
temp1 = number;
/*Convert to float */
temp_float1 = number * 3.051757812500000e-005f;
/*Store as integer */
tempconv.floatval = temp_float1;
bits_val1 = tempconv.fracval;
/* Subtract the shifted value from the magic number to give intial guess */
bits_val1 = 0x5f3759df - (bits_val1 >> 1); // gives initial guess
/* Store as float */
tempconv.fracval = bits_val1;
temp_float1 = tempconv.floatval;
/* Convert to integer format */
var1 = (q31_t) (temp_float1 * 16384);
/* 1st iteration */
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
((q15_t)
((((q15_t)
(((q31_t) var1 * var1) >> 15)) *
(q31_t) half) >> 15))) >> 15)) << 2;
/* 2nd iteration */
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
((q15_t)
((((q15_t)
(((q31_t) var1 * var1) >> 15)) *
(q31_t) half) >> 15))) >> 15)) << 2;
/* 3rd iteration */
var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
((q15_t)
((((q15_t)
(((q31_t) var1 * var1) >> 15)) *
(q31_t) half) >> 15))) >> 15)) << 2;
/* Multiply the inverse square root with the original value */
var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
/* Shift the output down accordingly */
if((signBits1 % 2) == 0)
{
var1 = var1 >> (signBits1 / 2);
}
else
{
var1 = var1 >> ((signBits1 - 1) / 2);
}
*pOut = var1;
return (ARM_MATH_SUCCESS);
}
/* If the number is a negative number then store zero as its square root value */
else
{
*pOut = 0;
return (ARM_MATH_ARGUMENT_ERROR);
}
}
/**
* @} end of SQRT group
*/
|
