Laxmi Kant Tiwari / mbed-dsp

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of mbed-dsp by mbed official

cmsis_dsp/FastMathFunctions/arm_sqrt_q31.c@1:fdd22bb7aa52, 2012-11-28 (annotated)

Committer:
emilmont
Date:
Wed Nov 28 12:30:09 2012 +0000
Revision:
1:fdd22bb7aa52
Child:
2:da51fb522205
DSP library code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
emilmont 1:fdd22bb7aa52 1 /* ----------------------------------------------------------------------
emilmont 1:fdd22bb7aa52 2 * Copyright (C) 2011 ARM Limited. All rights reserved.
emilmont 1:fdd22bb7aa52 3 *
emilmont 1:fdd22bb7aa52 4 * $Date: 15. February 2012
emilmont 1:fdd22bb7aa52 5 * $Revision: V1.1.0
emilmont 1:fdd22bb7aa52 6 *
emilmont 1:fdd22bb7aa52 7 * Project: CMSIS DSP Library
emilmont 1:fdd22bb7aa52 8 * Title: arm_sqrt_q31.c
emilmont 1:fdd22bb7aa52 9 *
emilmont 1:fdd22bb7aa52 10 * Description: Q31 square root function.
emilmont 1:fdd22bb7aa52 11 *
emilmont 1:fdd22bb7aa52 12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
emilmont 1:fdd22bb7aa52 13 *
emilmont 1:fdd22bb7aa52 14 * Version 1.1.0 2012/02/15
emilmont 1:fdd22bb7aa52 15 * Updated with more optimizations, bug fixes and minor API changes.
emilmont 1:fdd22bb7aa52 16 *
emilmont 1:fdd22bb7aa52 17 * Version 1.0.0 2011/03/08
emilmont 1:fdd22bb7aa52 18 * Alpha release.
emilmont 1:fdd22bb7aa52 19 *
emilmont 1:fdd22bb7aa52 20 * Version 1.0.1 2011/09/30
emilmont 1:fdd22bb7aa52 21 * Beta release.
emilmont 1:fdd22bb7aa52 22 *
emilmont 1:fdd22bb7aa52 23 * -------------------------------------------------------------------- */
emilmont 1:fdd22bb7aa52 24 #include "arm_math.h"
emilmont 1:fdd22bb7aa52 25 #include "arm_common_tables.h"
emilmont 1:fdd22bb7aa52 26
emilmont 1:fdd22bb7aa52 27 /**
emilmont 1:fdd22bb7aa52 28 * @ingroup groupFastMath
emilmont 1:fdd22bb7aa52 29 */
emilmont 1:fdd22bb7aa52 30
emilmont 1:fdd22bb7aa52 31 /**
emilmont 1:fdd22bb7aa52 32 * @addtogroup SQRT
emilmont 1:fdd22bb7aa52 33 * @{
emilmont 1:fdd22bb7aa52 34 */
emilmont 1:fdd22bb7aa52 35
emilmont 1:fdd22bb7aa52 36 /**
emilmont 1:fdd22bb7aa52 37 * @brief Q31 square root function.
emilmont 1:fdd22bb7aa52 38 * @param[in] in input value. The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
emilmont 1:fdd22bb7aa52 39 * @param[out] *pOut square root of input value.
emilmont 1:fdd22bb7aa52 40 * @return The function returns ARM_MATH_SUCCESS if input value is positive value or ARM_MATH_ARGUMENT_ERROR if
emilmont 1:fdd22bb7aa52 41 * <code>in</code> is negative value and returns zero output for negative values.
emilmont 1:fdd22bb7aa52 42 */
emilmont 1:fdd22bb7aa52 43
emilmont 1:fdd22bb7aa52 44 arm_status arm_sqrt_q31(
emilmont 1:fdd22bb7aa52 45 q31_t in,
emilmont 1:fdd22bb7aa52 46 q31_t * pOut)
emilmont 1:fdd22bb7aa52 47 {
emilmont 1:fdd22bb7aa52 48 q31_t number, temp1, bits_val1, var1, signBits1, half;
emilmont 1:fdd22bb7aa52 49 float32_t temp_float1;
emilmont 1:fdd22bb7aa52 50
emilmont 1:fdd22bb7aa52 51 number = in;
emilmont 1:fdd22bb7aa52 52
emilmont 1:fdd22bb7aa52 53 /* If the input is a positive number then compute the signBits. */
emilmont 1:fdd22bb7aa52 54 if(number > 0)
emilmont 1:fdd22bb7aa52 55 {
emilmont 1:fdd22bb7aa52 56 signBits1 = __CLZ(number) - 1;
emilmont 1:fdd22bb7aa52 57
emilmont 1:fdd22bb7aa52 58 /* Shift by the number of signBits1 */
emilmont 1:fdd22bb7aa52 59 if((signBits1 % 2) == 0)
emilmont 1:fdd22bb7aa52 60 {
emilmont 1:fdd22bb7aa52 61 number = number << signBits1;
emilmont 1:fdd22bb7aa52 62 }
emilmont 1:fdd22bb7aa52 63 else
emilmont 1:fdd22bb7aa52 64 {
emilmont 1:fdd22bb7aa52 65 number = number << (signBits1 - 1);
emilmont 1:fdd22bb7aa52 66 }
emilmont 1:fdd22bb7aa52 67
emilmont 1:fdd22bb7aa52 68 /* Calculate half value of the number */
emilmont 1:fdd22bb7aa52 69 half = number >> 1;
emilmont 1:fdd22bb7aa52 70 /* Store the number for later use */
emilmont 1:fdd22bb7aa52 71 temp1 = number;
emilmont 1:fdd22bb7aa52 72
emilmont 1:fdd22bb7aa52 73 /*Convert to float */
emilmont 1:fdd22bb7aa52 74 temp_float1 = number * 4.6566128731e-010f;
emilmont 1:fdd22bb7aa52 75 /*Store as integer */
emilmont 1:fdd22bb7aa52 76 bits_val1 = *(int *) &temp_float1;
emilmont 1:fdd22bb7aa52 77 /* Subtract the shifted value from the magic number to give intial guess */
emilmont 1:fdd22bb7aa52 78 bits_val1 = 0x5f3759df - (bits_val1 >> 1); // gives initial guess
emilmont 1:fdd22bb7aa52 79 /* Store as float */
emilmont 1:fdd22bb7aa52 80 temp_float1 = *(float *) &bits_val1;
emilmont 1:fdd22bb7aa52 81 /* Convert to integer format */
emilmont 1:fdd22bb7aa52 82 var1 = (q31_t) (temp_float1 * 1073741824);
emilmont 1:fdd22bb7aa52 83
emilmont 1:fdd22bb7aa52 84 /* 1st iteration */
emilmont 1:fdd22bb7aa52 85 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
emilmont 1:fdd22bb7aa52 86 ((q31_t)
emilmont 1:fdd22bb7aa52 87 ((((q31_t)
emilmont 1:fdd22bb7aa52 88 (((q63_t) var1 * var1) >> 31)) *
emilmont 1:fdd22bb7aa52 89 (q63_t) half) >> 31))) >> 31)) << 2;
emilmont 1:fdd22bb7aa52 90 /* 2nd iteration */
emilmont 1:fdd22bb7aa52 91 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
emilmont 1:fdd22bb7aa52 92 ((q31_t)
emilmont 1:fdd22bb7aa52 93 ((((q31_t)
emilmont 1:fdd22bb7aa52 94 (((q63_t) var1 * var1) >> 31)) *
emilmont 1:fdd22bb7aa52 95 (q63_t) half) >> 31))) >> 31)) << 2;
emilmont 1:fdd22bb7aa52 96 /* 3rd iteration */
emilmont 1:fdd22bb7aa52 97 var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
emilmont 1:fdd22bb7aa52 98 ((q31_t)
emilmont 1:fdd22bb7aa52 99 ((((q31_t)
emilmont 1:fdd22bb7aa52 100 (((q63_t) var1 * var1) >> 31)) *
emilmont 1:fdd22bb7aa52 101 (q63_t) half) >> 31))) >> 31)) << 2;
emilmont 1:fdd22bb7aa52 102
emilmont 1:fdd22bb7aa52 103 /* Multiply the inverse square root with the original value */
emilmont 1:fdd22bb7aa52 104 var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
emilmont 1:fdd22bb7aa52 105
emilmont 1:fdd22bb7aa52 106 /* Shift the output down accordingly */
emilmont 1:fdd22bb7aa52 107 if((signBits1 % 2) == 0)
emilmont 1:fdd22bb7aa52 108 {
emilmont 1:fdd22bb7aa52 109 var1 = var1 >> (signBits1 / 2);
emilmont 1:fdd22bb7aa52 110 }
emilmont 1:fdd22bb7aa52 111 else
emilmont 1:fdd22bb7aa52 112 {
emilmont 1:fdd22bb7aa52 113 var1 = var1 >> ((signBits1 - 1) / 2);
emilmont 1:fdd22bb7aa52 114 }
emilmont 1:fdd22bb7aa52 115 *pOut = var1;
emilmont 1:fdd22bb7aa52 116
emilmont 1:fdd22bb7aa52 117 return (ARM_MATH_SUCCESS);
emilmont 1:fdd22bb7aa52 118 }
emilmont 1:fdd22bb7aa52 119 /* If the number is a negative number then store zero as its square root value */
emilmont 1:fdd22bb7aa52 120 else
emilmont 1:fdd22bb7aa52 121 {
emilmont 1:fdd22bb7aa52 122 *pOut = 0;
emilmont 1:fdd22bb7aa52 123 return (ARM_MATH_ARGUMENT_ERROR);
emilmont 1:fdd22bb7aa52 124 }
emilmont 1:fdd22bb7aa52 125 }
emilmont 1:fdd22bb7aa52 126
emilmont 1:fdd22bb7aa52 127 /**
emilmont 1:fdd22bb7aa52 128 * @} end of SQRT group
emilmont 1:fdd22bb7aa52 129 */