arm_sqrt_q15.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_q15.c      
*      
* Description:  Q15 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_q15(     
  q15_t in,     
  q15_t * pOut)     
{  
    q15_t number, temp1, var1, signBits1, half;  
    q31_t bits_val1;  
    float32_t temp_float1;  
  
    number = in;  
  
    /* If the input is a positive number then compute the signBits. */  
    if(number > 0)  
    {  
      signBits1 = __CLZ(number) - 17;  
  
      /* 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 * 3.051757812500000e-005f;      
  
      /*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 * 16384);  
  
      /* 1st iteration */  
      var1 = ((q15_t)((q31_t) var1 * (0x3000 -  
             ((q15_t)((((q15_t) (((q31_t) var1 * var1) >> 15)) * (q31_t) half) >> 15))) >>15)) << 2;  
      /* 2nd iteration */  
      var1 = ((q15_t)((q31_t) var1 * (0x3000 -  
             ((q15_t)((((q15_t) (((q31_t) var1 * var1) >> 15)) * (q31_t) half) >> 15))) >> 15)) << 2;  
      /* 3rd iteration */  
      var1 = ((q15_t)((q31_t) var1 * (0x3000 -  
             ((q15_t)((((q15_t) (((q31_t) var1 * var1) >> 15)) * (q31_t) half) >> 15))) >> 15)) << 2;  
  
      /* Multiply the inverse square root with the original value */  
      var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 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);  
      }  
}