Guest User

Untitled

a guest
Oct 5th, 2012
75
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /*****************************************************************************
  2.  *
  3.  * \file
  4.  *
  5.  * \brief Square root operator for the AVR32 UC3.
  6.  *
  7.  * Copyright (c) 2009 Atmel Corporation. All rights reserved.
  8.  *
  9.  * \asf_license_start
  10.  *
  11.  * \page License
  12.  *
  13.  * Redistribution and use in source and binary forms, with or without
  14.  * modification, are permitted provided that the following conditions are met:
  15.  *
  16.  * 1. Redistributions of source code must retain the above copyright notice,
  17.  *    this list of conditions and the following disclaimer.
  18.  *
  19.  * 2. Redistributions in binary form must reproduce the above copyright notice,
  20.  *    this list of conditions and the following disclaimer in the documentation
  21.  *    and/or other materials provided with the distribution.
  22.  *
  23.  * 3. The name of Atmel may not be used to endorse or promote products derived
  24.  *    from this software without specific prior written permission.
  25.  *
  26.  * 4. This software may only be redistributed and used in connection with an
  27.  *    Atmel microcontroller product.
  28.  *
  29.  * THIS SOFTWARE IS PROVIDED BY ATMEL "AS IS" AND ANY EXPRESS OR IMPLIED
  30.  * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
  31.  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NON-INFRINGEMENT ARE
  32.  * EXPRESSLY AND SPECIFICALLY DISCLAIMED. IN NO EVENT SHALL ATMEL BE LIABLE FOR
  33.  * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  34.  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  35.  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  36.  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
  37.  * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  38.  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  39.  * POSSIBILITY OF SUCH DAMAGE.
  40.  *
  41.  * \asf_license_stop
  42.  *
  43.  *****************************************************************************/
  44.  
  45.  
  46. #include "dsp.h"
  47. #include "preprocessor.h"
  48.  
  49. #if defined(FORCE_ALL_GENERICS) || \
  50.     defined(FORCE_GENERIC_OP16_SQRT) || \
  51.     !defined(TARGET_SPECIFIC_OP16_SQRT)
  52.  
  53. #define DSP16_SQRT_ONE_POINT_FIVE   ((S32) (3 << (DSP16_QB-1)))
  54.  
  55. #define DSP16_SQRT_NEWTON_ITERATION(x_num, data) \
  56.   a = ((S32) x)*((S32) x); \
  57.   a = (((S32) num)*a) >> (DSP16_QB+1); \
  58.   x = (((S32) x)*(DSP16_SQRT_ONE_POINT_FIVE - a)) >> (DSP16_QB);
  59.  
  60. // Square root using Reciproot Iteration
  61. // Adapted for fixed point numbers
  62. dsp16_t dsp16_op_sqrt(dsp16_t num)
  63. {
  64.   int under_bit_val;
  65.   dsp16_t num_temp, x;
  66.   S32 a;
  67.  
  68.   // Limit
  69.   if (num < 0)
  70.     return 0;
  71.  
  72.   // Find an approximation of 1/sqrt(x);
  73.   under_bit_val = 0;
  74.   num_temp = num;
  75.   while(num_temp)
  76.   {
  77.     num_temp >>= 1;
  78.     under_bit_val++;
  79.   }
  80.   under_bit_val >>= 1;
  81.  
  82.   // x ~ 1/sqrt(num)
  83.   x = 1 << (DSP16_QB - under_bit_val);
  84.  
  85.   // Perform a Newton Iteration
  86.   MREPEAT(3, DSP16_SQRT_NEWTON_ITERATION, "");
  87.  
  88. #if (DSP16_QB%2 == 1)
  89.   // To support Q1.(2N+1) fixed point numbers
  90.   num = (num*((S32) DSP16_Q(CST_INV_SQUARE_ROOT_2))) >> (DSP16_QB);
  91. #endif
  92.   // Get sqrt(x) from 1/sqrt(x)
  93.   a = (((S32) x)*((S32) num));
  94.   // Adjust the result for fixed point format
  95.   a >>= (DSP16_QB >> 1);
  96.  
  97.   return (dsp16_t) a;
  98. }
  99.  
  100. #endif
RAW Paste Data