Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027 #include "arm_math.h"
00028 #include "arm_common_tables.h"
00029
00047 arm_status arm_sqrt_q31(
00048 q31_t in,
00049 q31_t * pOut)
00050 {
00051 q63_t prevOut;
00052 q31_t oneByOut;
00053 uint32_t signBits;
00054
00055 #ifndef ARM_MATH_CM0
00056
00057
00058
00059 q63_t out;
00060
00061 if(in > 0)
00062 {
00063
00064
00065
00066
00067 out = (in >> 1) + 0x3FFFFFFF;
00068
00069
00070
00071
00072 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00073
00074
00075 out = out >> 1u;
00076
00077
00078 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00079
00080
00081 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00082 prevOut = prevOut >> 1u;
00083 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00084
00085 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00086 out = out >> 1u;
00087 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00088
00089
00090 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00091 prevOut = prevOut >> 1u;
00092 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00093
00094 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00095 out = out >> 1u;
00096 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00097
00098
00099 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00100 prevOut = prevOut >> 1u;
00101 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00102
00103 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00104 out = out >> 1u;
00105 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00106
00107 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00108 prevOut = prevOut >> 1u;
00109 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00110
00111 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00112 out = out >> 1u;
00113 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00114
00115 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00116 prevOut = prevOut >> 1u;
00117 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00118
00119 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00120 out = out >> 1u;
00121 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00122
00123 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00124 prevOut = prevOut >> 1u;
00125 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00126
00127 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00128 out = out >> 1u;
00129 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00130
00131 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00132 prevOut = prevOut >> 1u;
00133 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00134
00135
00136 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00137 out = out >> 1u;
00138 *pOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00139
00140 return (ARM_MATH_SUCCESS);
00141 }
00142
00143 #else
00144
00145
00146
00147 q63_t out, loopVar;
00148 if(in > 0)
00149 {
00150
00151
00152
00153
00154 out = (in >> 1) + 0x3FFFFFFF;
00155
00156
00157
00158
00159 signBits = arm_recip_q31((q31_t) out, &oneByOut, armRecipTableQ31);
00160
00161
00162 out = out >> 1u;
00163
00164
00165 prevOut = out + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00166
00167
00168
00169
00170 for (loopVar = 1; loopVar <= 14; loopVar++)
00171 {
00172
00173 signBits = arm_recip_q31((q31_t) prevOut, &oneByOut, armRecipTableQ31);
00174
00175 prevOut = prevOut >> 1u;
00176
00177 out = prevOut + (((q31_t) (((q63_t) in * oneByOut) >> 32)) << signBits);
00178
00179 prevOut = out;
00180
00181 }
00182
00183 *pOut = prevOut;
00184
00185 return (ARM_MATH_SUCCESS);
00186 }
00187
00188 #endif
00189
00190 else
00191 {
00192 *pOut = 0;
00193 return (ARM_MATH_ARGUMENT_ERROR);
00194 }
00195 }
00196