1 /* 2 * Copyright (c) 2018 Vikrant More 3 * 4 * SPDX-License-Identifier: Apache-2.0 5 */ 6 7 #include <math.h> 8 #include <stdint.h> 9 #include <stdio.h> 10 #include <stdlib.h> 11 #include <zephyr/sys/util.h> 12 13 #define MAX_F_ITTERATIONS 6 /* usually converges in 4 loops */ 14 /* this ensures we break out of the loop */ 15 #define MAX_F_ERROR_COUNT 3 /* when result almost converges, stop */ 16 #define EXP_MASK32 GENMASK(30, 23) 17 sqrtf(float square)18float sqrtf(float square) 19 { 20 int i; 21 float root; 22 float last; 23 int32_t exponent; 24 int32_t *p_square = (int32_t *)□ 25 int32_t *p_root = (int32_t *)&root; 26 int32_t *p_last = (int32_t *)&last; 27 28 if (square == 0.0f) { 29 return square; 30 } 31 if (square < 0.0f) { 32 return (square - square) / (square - square); 33 } 34 35 /* we need a good starting guess so that this will converge quickly, 36 * we can do this by dividing the exponent part of the float by 2 37 * this assumes IEEE-754 format doubles 38 */ 39 exponent = ((*p_square & EXP_MASK32)>>23)-127; 40 if (exponent == 0xFF-127) { 41 /* the number is a NAN or inf, return NaN or inf */ 42 return square + square; 43 } 44 exponent /= 2; 45 *p_root = (*p_square & ~EXP_MASK32) | (exponent+127) << 23; 46 47 for (i = 0; i < MAX_F_ITTERATIONS; i++) { 48 last = root; 49 root = (root + square / root) * 0.5f; 50 /* if (labs(*p_root - *p_last) < MAX_F_ERROR_COUNT) */ 51 if ((*p_root ^ *p_last) < MAX_F_ERROR_COUNT) { 52 break; 53 } 54 } 55 return root; 56 } 57