Robert Lopez / CMSIS5

Aded CMSIS5 DSP and NN folder. Needs some work

DSP/source/FastMathFunctions/arm_sqrt_q31.c@0:eedb7d567a5d, 2018-04-12 (annotated)

Committer:
robert_lp
Date:
Thu Apr 12 01:31:58 2018 +0000
Revision:
0:eedb7d567a5d
CMSIS5 Library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
robert_lp 0:eedb7d567a5d 1 /* ----------------------------------------------------------------------
robert_lp 0:eedb7d567a5d 2 * Project: CMSIS DSP Library
robert_lp 0:eedb7d567a5d 3 * Title: arm_sqrt_q31.c
robert_lp 0:eedb7d567a5d 4 * Description: Q31 square root function
robert_lp 0:eedb7d567a5d 5 *
robert_lp 0:eedb7d567a5d 6 * $Date: 27. January 2017
robert_lp 0:eedb7d567a5d 7 * $Revision: V.1.5.1
robert_lp 0:eedb7d567a5d 8 *
robert_lp 0:eedb7d567a5d 9 * Target Processor: Cortex-M cores
robert_lp 0:eedb7d567a5d 10 * -------------------------------------------------------------------- */
robert_lp 0:eedb7d567a5d 11 /*
robert_lp 0:eedb7d567a5d 12 * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved.
robert_lp 0:eedb7d567a5d 13 *
robert_lp 0:eedb7d567a5d 14 * SPDX-License-Identifier: Apache-2.0
robert_lp 0:eedb7d567a5d 15 *
robert_lp 0:eedb7d567a5d 16 * Licensed under the Apache License, Version 2.0 (the License); you may
robert_lp 0:eedb7d567a5d 17 * not use this file except in compliance with the License.
robert_lp 0:eedb7d567a5d 18 * You may obtain a copy of the License at
robert_lp 0:eedb7d567a5d 19 *
robert_lp 0:eedb7d567a5d 20 * www.apache.org/licenses/LICENSE-2.0
robert_lp 0:eedb7d567a5d 21 *
robert_lp 0:eedb7d567a5d 22 * Unless required by applicable law or agreed to in writing, software
robert_lp 0:eedb7d567a5d 23 * distributed under the License is distributed on an AS IS BASIS, WITHOUT
robert_lp 0:eedb7d567a5d 24 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
robert_lp 0:eedb7d567a5d 25 * See the License for the specific language governing permissions and
robert_lp 0:eedb7d567a5d 26 * limitations under the License.
robert_lp 0:eedb7d567a5d 27 */
robert_lp 0:eedb7d567a5d 28
robert_lp 0:eedb7d567a5d 29 #include "arm_math.h"
robert_lp 0:eedb7d567a5d 30 #include "arm_common_tables.h"
robert_lp 0:eedb7d567a5d 31
robert_lp 0:eedb7d567a5d 32 /**
robert_lp 0:eedb7d567a5d 33 * @ingroup groupFastMath
robert_lp 0:eedb7d567a5d 34 */
robert_lp 0:eedb7d567a5d 35
robert_lp 0:eedb7d567a5d 36 /**
robert_lp 0:eedb7d567a5d 37 * @addtogroup SQRT
robert_lp 0:eedb7d567a5d 38 * @{
robert_lp 0:eedb7d567a5d 39 */
robert_lp 0:eedb7d567a5d 40
robert_lp 0:eedb7d567a5d 41 /**
robert_lp 0:eedb7d567a5d 42 * @brief Q31 square root function.
robert_lp 0:eedb7d567a5d 43 * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
robert_lp 0:eedb7d567a5d 44 * @param[out] *pOut square root of input value.
robert_lp 0:eedb7d567a5d 45 * @return The function returns ARM_MATH_SUCCESS if the input value is positive
robert_lp 0:eedb7d567a5d 46 * and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
robert_lp 0:eedb7d567a5d 47 * negative inputs, the function returns *pOut = 0.
robert_lp 0:eedb7d567a5d 48 */
robert_lp 0:eedb7d567a5d 49
robert_lp 0:eedb7d567a5d 50 arm_status arm_sqrt_q31(
robert_lp 0:eedb7d567a5d 51 q31_t in,
robert_lp 0:eedb7d567a5d 52 q31_t * pOut)
robert_lp 0:eedb7d567a5d 53 {
robert_lp 0:eedb7d567a5d 54 q31_t number, temp1, bits_val1, var1, signBits1, half;
robert_lp 0:eedb7d567a5d 55 float32_t temp_float1;
robert_lp 0:eedb7d567a5d 56 union
robert_lp 0:eedb7d567a5d 57 {
robert_lp 0:eedb7d567a5d 58 q31_t fracval;
robert_lp 0:eedb7d567a5d 59 float32_t floatval;
robert_lp 0:eedb7d567a5d 60 } tempconv;
robert_lp 0:eedb7d567a5d 61
robert_lp 0:eedb7d567a5d 62 number = in;
robert_lp 0:eedb7d567a5d 63
robert_lp 0:eedb7d567a5d 64 /* If the input is a positive number then compute the signBits. */
robert_lp 0:eedb7d567a5d 65 if (number > 0)
robert_lp 0:eedb7d567a5d 66 {
robert_lp 0:eedb7d567a5d 67 signBits1 = __CLZ(number) - 1;
robert_lp 0:eedb7d567a5d 68
robert_lp 0:eedb7d567a5d 69 /* Shift by the number of signBits1 */
robert_lp 0:eedb7d567a5d 70 if ((signBits1 % 2) == 0)
robert_lp 0:eedb7d567a5d 71 {
robert_lp 0:eedb7d567a5d 72 number = number << signBits1;
robert_lp 0:eedb7d567a5d 73 }
robert_lp 0:eedb7d567a5d 74 else
robert_lp 0:eedb7d567a5d 75 {
robert_lp 0:eedb7d567a5d 76 number = number << (signBits1 - 1);
robert_lp 0:eedb7d567a5d 77 }
robert_lp 0:eedb7d567a5d 78
robert_lp 0:eedb7d567a5d 79 /* Calculate half value of the number */
robert_lp 0:eedb7d567a5d 80 half = number >> 1;
robert_lp 0:eedb7d567a5d 81 /* Store the number for later use */
robert_lp 0:eedb7d567a5d 82 temp1 = number;
robert_lp 0:eedb7d567a5d 83
robert_lp 0:eedb7d567a5d 84 /*Convert to float */
robert_lp 0:eedb7d567a5d 85 temp_float1 = number * 4.6566128731e-010f;
robert_lp 0:eedb7d567a5d 86 /*Store as integer */
robert_lp 0:eedb7d567a5d 87 tempconv.floatval = temp_float1;
robert_lp 0:eedb7d567a5d 88 bits_val1 = tempconv.fracval;
robert_lp 0:eedb7d567a5d 89 /* Subtract the shifted value from the magic number to give intial guess */
robert_lp 0:eedb7d567a5d 90 bits_val1 = 0x5f3759df - (bits_val1 >> 1); /* gives initial guess */
robert_lp 0:eedb7d567a5d 91 /* Store as float */
robert_lp 0:eedb7d567a5d 92 tempconv.fracval = bits_val1;
robert_lp 0:eedb7d567a5d 93 temp_float1 = tempconv.floatval;
robert_lp 0:eedb7d567a5d 94 /* Convert to integer format */
robert_lp 0:eedb7d567a5d 95 var1 = (q31_t) (temp_float1 * 1073741824);
robert_lp 0:eedb7d567a5d 96
robert_lp 0:eedb7d567a5d 97 /* 1st iteration */
robert_lp 0:eedb7d567a5d 98 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
robert_lp 0:eedb7d567a5d 99 ((q31_t)
robert_lp 0:eedb7d567a5d 100 ((((q31_t)
robert_lp 0:eedb7d567a5d 101 (((q63_t) var1 * var1) >> 31)) *
robert_lp 0:eedb7d567a5d 102 (q63_t) half) >> 31))) >> 31)) << 2;
robert_lp 0:eedb7d567a5d 103 /* 2nd iteration */
robert_lp 0:eedb7d567a5d 104 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
robert_lp 0:eedb7d567a5d 105 ((q31_t)
robert_lp 0:eedb7d567a5d 106 ((((q31_t)
robert_lp 0:eedb7d567a5d 107 (((q63_t) var1 * var1) >> 31)) *
robert_lp 0:eedb7d567a5d 108 (q63_t) half) >> 31))) >> 31)) << 2;
robert_lp 0:eedb7d567a5d 109 /* 3rd iteration */
robert_lp 0:eedb7d567a5d 110 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
robert_lp 0:eedb7d567a5d 111 ((q31_t)
robert_lp 0:eedb7d567a5d 112 ((((q31_t)
robert_lp 0:eedb7d567a5d 113 (((q63_t) var1 * var1) >> 31)) *
robert_lp 0:eedb7d567a5d 114 (q63_t) half) >> 31))) >> 31)) << 2;
robert_lp 0:eedb7d567a5d 115
robert_lp 0:eedb7d567a5d 116 /* Multiply the inverse square root with the original value */
robert_lp 0:eedb7d567a5d 117 var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
robert_lp 0:eedb7d567a5d 118
robert_lp 0:eedb7d567a5d 119 /* Shift the output down accordingly */
robert_lp 0:eedb7d567a5d 120 if ((signBits1 % 2) == 0)
robert_lp 0:eedb7d567a5d 121 {
robert_lp 0:eedb7d567a5d 122 var1 = var1 >> (signBits1 / 2);
robert_lp 0:eedb7d567a5d 123 }
robert_lp 0:eedb7d567a5d 124 else
robert_lp 0:eedb7d567a5d 125 {
robert_lp 0:eedb7d567a5d 126 var1 = var1 >> ((signBits1 - 1) / 2);
robert_lp 0:eedb7d567a5d 127 }
robert_lp 0:eedb7d567a5d 128 *pOut = var1;
robert_lp 0:eedb7d567a5d 129
robert_lp 0:eedb7d567a5d 130 return (ARM_MATH_SUCCESS);
robert_lp 0:eedb7d567a5d 131 }
robert_lp 0:eedb7d567a5d 132 /* If the number is a negative number then store zero as its square root value */
robert_lp 0:eedb7d567a5d 133 else
robert_lp 0:eedb7d567a5d 134 {
robert_lp 0:eedb7d567a5d 135 *pOut = 0;
robert_lp 0:eedb7d567a5d 136 return (ARM_MATH_ARGUMENT_ERROR);
robert_lp 0:eedb7d567a5d 137 }
robert_lp 0:eedb7d567a5d 138 }
robert_lp 0:eedb7d567a5d 139
robert_lp 0:eedb7d567a5d 140 /**
robert_lp 0:eedb7d567a5d 141 * @} end of SQRT group
robert_lp 0:eedb7d567a5d 142 */
robert_lp 0:eedb7d567a5d 143