CMSIS DSP Lib
Fork of mbed-dsp by
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?
User | Revision | Line number | New 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 | */ |