arm_sqrt_q31.c

Go to the documentation of this file.

/* ----------------------------------------------------------------------      
* Copyright (C) 2011 ARM Limited. All rights reserved. 
*      
* $Date:        15. December 2011   
* $Revision:    V2.0.0  
*      
* Project:      Cortex-R DSP Library 
* Title:        arm_sqrt_q31.c      
*      
* Description:  Q31 square root function.     
*      
* Target Processor:          Cortex-R4/R5
*
* Version 1.0.0 2011/03/08
*     Alpha release.
*
* Version 1.0.1 2011/09/30
*     Beta release.
*
* Version 2.0.0 2011/12/15
*     Final release. 
* 
* -------------------------------------------------------------------- */     
#include "arm_math.h"     
#include "arm_common_tables.h"     
     
arm_status arm_sqrt_q31(     
  q31_t in,     
  q31_t * pOut)     
{  
    q31_t number, temp1, bits_val1, var1, signBits1, half;  
    float32_t temp_float1;  
  
    number = in;  
  
    /* If the input is a positive number then compute the signBits. */  
    if(number > 0)  
    {  
      signBits1 = __CLZ(number) - 1;  
  
      /* Shift by the number of signBits1 */  
      if((signBits1 % 2) == 0)  
      {  
        number = number << signBits1;  
      }  
      else  
      {  
        number = number << (signBits1 - 1);  
      }  
  
      /* Calculate half value of the number */  
      half = number >> 1;  
  
      /* Store the number for later use */  
      temp1 = number;  
  
      /*Convert to float */  
      temp_float1 = number * 4.6566128731e-010f;  
  
      /*Store as integer */  
      bits_val1 = *(int *) &temp_float1;  
  
      /* Subtract the shifted value from the magic number to give intial guess */  
      bits_val1 = 0x5f3759df - (bits_val1 >> 1);        // gives initial guess   
  
      /* Store as float */  
      temp_float1 = *(float *) &bits_val1;  
  
      /* Convert to integer format */  
      var1 = (q31_t) (temp_float1 * 1073741824);  
  
      /* 1st iteration */  
      var1 = ((q31_t)((q63_t) var1 * (0x30000000 -  
             ((q31_t)((((q31_t) (((q63_t) var1 * var1) >> 31)) * (q63_t) half) >> 31))) >> 31)) << 2;  
      /* 2nd iteration */  
      var1 = ((q31_t)((q63_t) var1 * (0x30000000 -  
             ((q31_t)((((q31_t) (((q63_t) var1 * var1) >> 31)) * (q63_t) half) >> 31))) >> 31)) << 2;  
      /* 3rd iteration */  
      var1 = ((q31_t)((q63_t) var1 * (0x30000000 -  
             ((q31_t)((((q31_t) (((q63_t) var1 * var1) >> 31)) * (q63_t) half) >> 31))) >> 31)) << 2;  
  
      /* Multiply the inverse square root with the original value */  
      var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;  
  
      /* Shift the output down accordingly */  
      if((signBits1 % 2) == 0)  
      {  
        var1 = var1 >> (signBits1 / 2);  
      }  
      else  
      {  
        var1 = var1 >> ((signBits1 - 1) / 2);  
      }  
  
      *pOut = var1;  
  
      return (ARM_MATH_SUCCESS);  
    }  
    /* If the number is a negative number then store zero as its square root value */  
    else  
    {  
      *pOut = 0;     
      return (ARM_MATH_ARGUMENT_ERROR);  
      }  
}