123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155 |
- #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;
- union
- {
- q31_t fracval;
- float32_t floatval;
- } tempconv;
- number = in;
-
- if(number > 0)
- {
- signBits1 = __CLZ(number) - 17;
-
- if((signBits1 % 2) == 0)
- {
- number = number << signBits1;
- }
- else
- {
- number = number << (signBits1 - 1);
- }
-
- half = number >> 1;
-
- temp1 = number;
-
- temp_float1 = number * 3.051757812500000e-005f;
-
- tempconv.floatval = temp_float1;
- bits_val1 = tempconv.fracval;
-
- bits_val1 = 0x5f3759df - (bits_val1 >> 1);
-
- tempconv.fracval = bits_val1;
- temp_float1 = tempconv.floatval;
-
- var1 = (q31_t) (temp_float1 * 16384);
-
- var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
- ((q15_t)
- ((((q15_t)
- (((q31_t) var1 * var1) >> 15)) *
- (q31_t) half) >> 15))) >> 15)) << 2;
-
- var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
- ((q15_t)
- ((((q15_t)
- (((q31_t) var1 * var1) >> 15)) *
- (q31_t) half) >> 15))) >> 15)) << 2;
-
- var1 = ((q15_t) ((q31_t) var1 * (0x3000 -
- ((q15_t)
- ((((q15_t)
- (((q31_t) var1 * var1) >> 15)) *
- (q31_t) half) >> 15))) >> 15)) << 2;
-
- var1 = ((q15_t) (((q31_t) temp1 * var1) >> 15)) << 1;
-
- if((signBits1 % 2) == 0)
- {
- var1 = var1 >> (signBits1 / 2);
- }
- else
- {
- var1 = var1 >> ((signBits1 - 1) / 2);
- }
- *pOut = var1;
- return (ARM_MATH_SUCCESS);
- }
-
- else
- {
- *pOut = 0;
- return (ARM_MATH_ARGUMENT_ERROR);
- }
- }
|