00001 /*This file has been prepared for Doxygen automatic documentation generation.*/ 00015 /* Copyright (c) 2009 Atmel Corporation. All rights reserved. 00016 * 00017 * Redistribution and use in source and binary forms, with or without 00018 * modification, are permitted provided that the following conditions are met: 00019 * 00020 * 1. Redistributions of source code must retain the above copyright notice, this 00021 * list of conditions and the following disclaimer. 00022 * 00023 * 2. Redistributions in binary form must reproduce the above copyright notice, 00024 * this list of conditions and the following disclaimer in the documentation 00025 * and/or other materials provided with the distribution. 00026 * 00027 * 3. The name of Atmel may not be used to endorse or promote products derived 00028 * from this software without specific prior written permission. 00029 * 00030 * 4. This software may only be redistributed and used in connection with an Atmel 00031 * AVR product. 00032 * 00033 * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED 00034 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 00035 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE 00036 * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR 00037 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00038 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00039 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00040 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00041 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00042 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE 00043 * 00044 */ 00045 #include "dsp.h" 00046 #include "preprocessor.h" 00047 00048 #if defined(FORCE_ALL_GENERICS) || \ 00049 defined(FORCE_GENERIC_OP32_SQRT) || \ 00050 !defined(TARGET_SPECIFIC_OP32_SQRT) 00051 00052 #define DSP32_SQRT_ONE_POINT_FIVE ((S64) (3LL << (DSP32_QB-1))) 00053 00054 #define DSP32_SQRT_NEWTON_ITERATION(x_num, data) \ 00055 a = (x*x); \ 00056 a = (s64_num*a) >> (DSP32_QB+1); \ 00057 x = (x*(DSP32_SQRT_ONE_POINT_FIVE - a)) >> (DSP32_QB); 00058 00059 // Square root using Reciproot Iteration 00060 // Adapted for fixed point numbers 00061 dsp32_t dsp32_op_sqrt(dsp32_t num) 00062 { 00063 int under_bit_val; 00064 dsp32_t num_temp; 00065 S64 a, x, s64_num; 00066 00067 // Limit 00068 if (num < 0) 00069 return 0; 00070 00071 // Find an approximation of 1/sqrt(x); 00072 under_bit_val = 0; 00073 num_temp = num; 00074 while(num_temp) 00075 { 00076 num_temp >>= 1; 00077 under_bit_val++; 00078 } 00079 under_bit_val >>= 1; 00080 00081 // x ~ 1/sqrt(num) 00082 x = 1 << (DSP32_QB - under_bit_val); 00083 00084 s64_num = num; 00085 // Perform a Newton Iteration 00086 MREPEAT(6, DSP32_SQRT_NEWTON_ITERATION, ""); 00087 00088 #if (DSP32_QB%2 == 1) 00089 // To support Q1.(2N+1) fixed point numbers 00090 s64_num = (s64_num*((S64) DSP32_Q(CST_INV_SQUARE_ROOT_2))) >> (DSP32_QB); 00091 #endif 00092 // Get sqrt(x) from 1/sqrt(x) 00093 a = x*s64_num; 00094 00095 // Adjust the result for fixed point format 00096 a >>= (DSP32_QB >> 1); 00097 00098 return (dsp32_t) a; 00099 } 00100 00101 #endif