Robert Lopez / CMSIS5

Aded CMSIS5 DSP and NN folder. Needs some work

DSP/source/FastMathFunctions/arm_sqrt_q15.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_q15.c
robert_lp 0:eedb7d567a5d 4 * Description: Q15 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 /**
robert_lp 0:eedb7d567a5d 34 * @ingroup groupFastMath
robert_lp 0:eedb7d567a5d 35 */
robert_lp 0:eedb7d567a5d 36
robert_lp 0:eedb7d567a5d 37 /**
robert_lp 0:eedb7d567a5d 38 * @addtogroup SQRT
robert_lp 0:eedb7d567a5d 39 * @{
robert_lp 0:eedb7d567a5d 40 */
robert_lp 0:eedb7d567a5d 41
robert_lp 0:eedb7d567a5d 42 /**
robert_lp 0:eedb7d567a5d 43 * @brief Q15 square root function.
robert_lp 0:eedb7d567a5d 44 * @param[in] in input value. The range of the input value is [0 +1) or 0x0000 to 0x7FFF.
robert_lp 0:eedb7d567a5d 45 * @param[out] *pOut square root of input value.
robert_lp 0:eedb7d567a5d 46 * @return The function returns ARM_MATH_SUCCESS if the input value is positive
robert_lp 0:eedb7d567a5d 47 * and ARM_MATH_ARGUMENT_ERROR if the input is negative. For
robert_lp 0:eedb7d567a5d 48 * negative inputs, the function returns *pOut = 0.
robert_lp 0:eedb7d567a5d 49 */
robert_lp 0:eedb7d567a5d 50
robert_lp 0:eedb7d567a5d 51 arm_status arm_sqrt_q15(
robert_lp 0:eedb7d567a5d 52 q15_t in,
robert_lp 0:eedb7d567a5d 53 q15_t * pOut)
robert_lp 0:eedb7d567a5d 54 {
robert_lp 0:eedb7d567a5d 55 q15_t number, temp1, var1, signBits1, half;
robert_lp 0:eedb7d567a5d 56 q31_t bits_val1;
robert_lp 0:eedb7d567a5d 57 float32_t temp_float1;
robert_lp 0:eedb7d567a5d 58 union
robert_lp 0:eedb7d567a5d 59 {
robert_lp 0:eedb7d567a5d 60 q31_t fracval;
robert_lp 0:eedb7d567a5d 61 float32_t floatval;
robert_lp 0:eedb7d567a5d 62 } tempconv;
robert_lp 0:eedb7d567a5d 63
robert_lp 0:eedb7d567a5d 64 number = in;
robert_lp 0:eedb7d567a5d 65
robert_lp 0:eedb7d567a5d 66 /* If the input is a positive number then compute the signBits. */
robert_lp 0:eedb7d567a5d 67 if (number > 0)
robert_lp 0:eedb7d567a5d 68 {
robert_lp 0:eedb7d567a5d 69 signBits1 = __CLZ(number) - 17;
robert_lp 0:eedb7d567a5d 70
robert_lp 0:eedb7d567a5d 71 /* Shift by the number of signBits1 */
robert_lp 0:eedb7d567a5d 72 if ((signBits1 % 2) == 0)
robert_lp 0:eedb7d567a5d 73 {
robert_lp 0:eedb7d567a5d 74 number = number << signBits1;
robert_lp 0:eedb7d567a5d 75 }
robert_lp 0:eedb7d567a5d 76 else
robert_lp 0:eedb7d567a5d 77 {
robert_lp 0:eedb7d567a5d 78 number = number << (signBits1 - 1);
robert_lp 0:eedb7d567a5d 79 }
robert_lp 0:eedb7d567a5d 80
robert_lp 0:eedb7d567a5d 81 /* Calculate half value of the number */
robert_lp 0:eedb7d567a5d 82 half = number >> 1;
robert_lp 0:eedb7d567a5d 83 /* Store the number for later use */
robert_lp 0:eedb7d567a5d 84 temp1 = number;
robert_lp 0:eedb7d567a5d 85
robert_lp 0:eedb7d567a5d 86 /* Convert to float */
robert_lp 0:eedb7d567a5d 87 temp_float1 = number * 3.051757812500000e-005f;
robert_lp 0:eedb7d567a5d 88 /*Store as integer */
robert_lp 0:eedb7d567a5d 89 tempconv.floatval = temp_float1;
robert_lp 0:eedb7d567a5d 90 bits_val1 = tempconv.fracval;
robert_lp 0:eedb7d567a5d 91 /* Subtract the shifted value from the magic number to give intial guess */
robert_lp 0:eedb7d567a5d 92 bits_val1 = 0x5f3759df - (bits_val1 >> 1); /* gives initial guess */
robert_lp 0:eedb7d567a5d 93 /* Store as float */
robert_lp 0:eedb7d567a5d 94 tempconv.fracval = bits_val1;
robert_lp 0:eedb7d567a5d 95 temp_float1 = tempconv.floatval;
robert_lp 0:eedb7d567a5d 96 /* Convert to integer format */
robert_lp 0:eedb7d567a5d 97 var1 = (q31_t) (temp_float1 * 16384);
robert_lp 0:eedb7d567a5d 98
robert_lp 0:eedb7d567a5d 99 /* 1st iteration */
robert_lp 0:eedb7d567a5d 100 var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
robert_lp 0:eedb7d567a5d 101 ((q15_t)
robert_lp 0:eedb7d567a5d 102 ((((q15_t)
robert_lp 0:eedb7d567a5d 103 (((q31_t) var1 * var1) >> 15)) *
robert_lp 0:eedb7d567a5d 104 (q31_t) half) >> 15))) >> 15)) << 2;
robert_lp 0:eedb7d567a5d 105 /* 2nd iteration */
robert_lp 0:eedb7d567a5d 106 var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
robert_lp 0:eedb7d567a5d 107 ((q15_t)
robert_lp 0:eedb7d567a5d 108 ((((q15_t)
robert_lp 0:eedb7d567a5d 109 (((q31_t) var1 * var1) >> 15)) *
robert_lp 0:eedb7d567a5d 110 (q31_t) half) >> 15))) >> 15)) << 2;
robert_lp 0:eedb7d567a5d 111 /* 3rd iteration */
robert_lp 0:eedb7d567a5d 112 var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
robert_lp 0:eedb7d567a5d 113 ((q15_t)
robert_lp 0:eedb7d567a5d 114 ((((q15_t)
robert_lp 0:eedb7d567a5d 115 (((q31_t) var1 * var1) >> 15)) *
robert_lp 0:eedb7d567a5d 116 (q31_t) half) >> 15))) >> 15)) << 2;
robert_lp 0:eedb7d567a5d 117
robert_lp 0:eedb7d567a5d 118 /* Multiply the inverse square root with the original value */
robert_lp 0:eedb7d567a5d 119 var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
robert_lp 0:eedb7d567a5d 120
robert_lp 0:eedb7d567a5d 121 /* Shift the output down accordingly */
robert_lp 0:eedb7d567a5d 122 if ((signBits1 % 2) == 0)
robert_lp 0:eedb7d567a5d 123 {
robert_lp 0:eedb7d567a5d 124 var1 = var1 >> (signBits1 / 2);
robert_lp 0:eedb7d567a5d 125 }
robert_lp 0:eedb7d567a5d 126 else
robert_lp 0:eedb7d567a5d 127 {
robert_lp 0:eedb7d567a5d 128 var1 = var1 >> ((signBits1 - 1) / 2);
robert_lp 0:eedb7d567a5d 129 }
robert_lp 0:eedb7d567a5d 130 *pOut = var1;
robert_lp 0:eedb7d567a5d 131
robert_lp 0:eedb7d567a5d 132 return (ARM_MATH_SUCCESS);
robert_lp 0:eedb7d567a5d 133 }
robert_lp 0:eedb7d567a5d 134 /* If the number is a negative number then store zero as its square root value */
robert_lp 0:eedb7d567a5d 135 else
robert_lp 0:eedb7d567a5d 136 {
robert_lp 0:eedb7d567a5d 137 *pOut = 0;
robert_lp 0:eedb7d567a5d 138 return (ARM_MATH_ARGUMENT_ERROR);
robert_lp 0:eedb7d567a5d 139 }
robert_lp 0:eedb7d567a5d 140 }
robert_lp 0:eedb7d567a5d 141
robert_lp 0:eedb7d567a5d 142 /**
robert_lp 0:eedb7d567a5d 143 * @} end of SQRT group
robert_lp 0:eedb7d567a5d 144 */
robert_lp 0:eedb7d567a5d 145