/* * Copyright (c) 2019-2026, Dmitry (DiSlord) dislordlive@gmail.com * All rights reserved. * * This is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 3, or (at your option) * any later version. * * The software is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with GNU Radio; see the file COPYING. If not, write to * the Free Software Foundation, Inc., 51 Franklin Street, * Boston, MA 02110-1301, USA. */ #include "nanovna.h" #include /* * float has about 7.2 digits of precision * built sin table in range 0 to PI/2, use indexes from 0 to (1< SIN_TABLE_N #error "Need use bigger SIN table for this FFT/IFFT" #endif #ifdef ARM_MATH_CM4 // Use CORTEX M4 rbit instruction (reverse bit order in 32bit value) static uint32_t reverse_bits(uint32_t x, int n) { uint32_t result; __asm volatile ("rbit %0, %1" : "=r" (result) : "r" (x) ); return result>>(32-n); // made shift for correct result } #elif 0 // Use shifts static uint32_t reverse_bits(uint32_t x, int n) { // up to 16 bit x = ((x & 0x5555) << 1) | ((x & 0xAAAA) >> 1); x = ((x & 0x3333) << 2) | ((x & 0xCCCC) >> 2); x = ((x & 0x0F0F) << 4) | ((x & 0xF0F0) >> 4); x = ((x & 0x00FF) << 8) | ((x & 0xFF00) >> 8); return x>>(16-n); } #elif 1 // Use table static uint16_t reverse_bits(uint16_t x, int n) { // up to 12 bit static const uint8_t rev_nibble[16] = {0x0, 0x8, 0x4, 0xC, 0x2, 0xA, 0x6, 0xE, 0x1, 0x9, 0x3, 0xB, 0x5, 0xD, 0x7, 0xF}; x = (rev_nibble[(x >> 0) & 0xF] << 8) | (rev_nibble[(x >> 4) & 0xF] << 4) | (rev_nibble[(x >> 8) & 0xF] << 0); return x>>(12-n); } #else // Direct calculations static uint16_t reverse_bits(uint16_t x, int n) { uint16_t result = 0; int i; for (i = 0; i < n; i++, x >>= 1) result = (result << 1) | (x & 1U); return result; } #endif // Cooley-Tukey radix-2 DIT FFT, dir = 0:forward, 1:inverse void fft(float array[][2], const uint8_t dir) { uint16_t fft_n = FFT_N; // can be in range 1 <= fft_n <= SIN_TABLE_N uint16_t fft_size = 1< i) { SWAP(float, array[i][0], array[j][0]); SWAP(float, array[i][1], array[j][1]); } } // Optimized Cooley-Tukey decimation-in-time radix-2 FFT // Use SIN table (only first period), table size (1<>=1, size<<=1) { for (i = 0; i < size; i++) { uint32_t table_index = i * tablestep; const uint32_t SIN_TABLE_SUB = SIN_TABLE_N - 2; // SIN table one sector N (full table must contain 4 sectors) const uint32_t sector = table_index >> SIN_TABLE_SUB; const uint32_t sidx = table_index & ((1<> SIN_TABLE_SUB); // Get sector (0 to 3) mask: 0bXXXX for 3210 const uint32_t sidx = table_index & ((1<>23)&0xff) - 0x7f; // get exponent if (e < 0) { // no integral part if (iptr) *iptr = 0; return u.f; } if (e >= 23) x = 0; // no fractional part else { x = u.f; u.i&= ~(0x007fffff>>e); // remove fractional part from u x-= u.f; // calc fractional part } //if (iptr) *iptr = ((u.i&0x007fffff)|0x00800000)>>(23-e); // cut integer part from float as integer if (iptr) *iptr = u.f; // cut integer part from float as float return x; } //********************************************************************************** // square root //********************************************************************************** #if (__FPU_PRESENT == 0) && (__FPU_USED == 0) #if 1 // __ieee754_sqrtf, remove some check (NAN, inf, normalization), small code optimization to arm float vna_sqrtf(float x) { int32_t ix,s,q,m,t; uint32_t r; union {float f; uint32_t i;} u = {x}; ix = u.i; #if 0 // take care of Inf and NaN if((ix&0x7f800000)==0x7f800000) return x*x+x; // sqrt(NaN)=NaN, sqrt(+inf)=+inf, sqrt(-inf)=sNaN // take care if x < 0 if (ix < 0) return (x-x)/0.0f; #endif if (ix == 0) return 0.0f; m = (ix>>23); #if 0 // // normalize x if(m==0) { // subnormal x for(int i=0;(ix&0x00800000)==0;i++) ix<<=1; m -= i-1; } #endif m -= 127; // unbias exponent ix = (ix&0x007fffff)|0x00800000; // generate sqrt(x) bit by bit ix<<= (m&1) ? 2 : 1; // odd m, double x to make it even, and after multiple by 2 m >>= 1; // m = [m/2] q = s = 0; // q = sqrt(x) r = 0x01000000; // r = moving bit from right to left while(r!=0) { t = s+r; if(t<=ix) { s = t+r; ix -= t; q += r; } ix += ix; r>>=1; } // use floating add to find out rounding direction if(ix!=0) { if ((1.0f - 1e-30f) >= 1.0f) // trigger inexact flag. q += ((1.0f + 1e-30f) > 1.0f) ? 2 : (q&1); } ix = (q>>1)+0x3f000000; ix += (m <<23); u.i = ix; return u.f; } #else // Simple implementation, but slow if no FPU used, and not usable if used hardware FPU sqrtf float vna_sqrtf(float x) { union {float x; uint32_t i;} u = {x}; u.i = (1<<29) + (u.i >> 1) - (1<<22); // Two Babylonian Steps (simplified from:) // u.x = 0.5f * (u.x + x/u.x); // u.x = 0.5f * (u.x + x/u.x); u.x = u.x + x/u.x; u.x = 0.25f*u.x + x/u.x; return u.x; } #endif #endif //********************************************************************************** // Cube root //********************************************************************************** float vna_cbrtf(float x) { #if 1 static const uint32_t B1 = 709958130, // B1 = (127-127.0/3-0.03306235651)*2**23 B2 = 642849266; // B2 = (127-127.0/3-24/3-0.03306235651)*2**23 float r,T; union {float f; uint32_t i;} u = {x}; uint32_t hx = u.i & 0x7fffffff; // if (hx >= 0x7f800000) // cbrt(NaN,INF) is itself // return x + x; // rough cbrtf to 5 bits if (hx < 0x00800000) { // zero or subnormal? if (hx == 0) return x; // cbrt(+-0) is itself u.f = x*0x1p24f; hx = u.i & 0x7fffffff; hx = hx/3 + B2; } else hx = hx/3 + B1; u.i &= 0x80000000; u.i |= hx; // First step Newton iteration (solving t*t-x/t == 0) to 16 bits. T = u.f; r = T*T*T; T*= (x+x+r)/(x+r+r); // Second step Newton iteration to 47 bits. r = T*T*T; T*= (x+x+r)/(x+r+r); return T; #else if (x == 0) { // would otherwise return something like 4.257959840008151e-109 return 0; } float b = 1.0f; // use any value except 0 float last_b_1 = 0; float last_b_2 = 0; while (last_b_1 != b && last_b_2 != b) { last_b_1 = b; // b = (b + x / (b * b)) / 2; b = (2 * b + x / b / b) / 3; // for small numbers, as suggested by willywonka_dailyblah last_b_2 = b; // b = (b + x / (b * b)) / 2; b = (2 * b + x / b / b) / 3; //for small numbers, as suggested by willywonka_dailyblah } return b; #endif } //********************************************************************************** // logf //********************************************************************************** float vna_logf(float x) { union {float f; int32_t i;} u = {x}; if (u.i <= 0) return -1/0.0f; // if <=0 return -inf const float MULT = logf(2.0f); #if 0 // Give up to 0.0067 error const int log_2 = ((u.i >> 23) & 255) - 128; u.i = (u.i&0x007FFFFF) + 0x3F800000; u.f = ((-1.0f/3) * u.f + 2) * u.f - (2.0f/3); return (u.f + log_2) * MULT; #elif 1 // Give up to 6.1e-5 error union { int32_t i; float f; } mx = { (u.i & 0x007FFFFF) | 0x3f000000 }; return u.i * (MULT / (1 << 23)) - (124.225784301758f * MULT) - (1.497851252556f * MULT) * mx.f - (1.725635766983f * MULT) / (0.352076232433f + mx.f); #else // Give up to 4.768e-7 error float f, z, dk; static const float ln2_hi = 6.9313812256e-01, // 0x3f317180 ln2_lo = 9.0580006145e-06, // 0x3717f7d1 Lg0 = 2.0f, Lg1 = 0xaaaaaa.0p-24, // 0.66666662693 Lg2 = 0xccce13.0p-25, // 0.40000972152 Lg3 = 0x91e9ee.0p-25, // 0.28498786688 Lg4 = 0xf89e26.0p-26; // 0.24279078841 u.i += 0x3f800000 - 0x3f3504f3;// reduce x into [sqrt(2)/2, sqrt(2)] dk = (int)(u.i>>23) - 0x7f; u.i = (u.i&0x007fffff) + 0x3f3504f3; f = (u.f - 1.0f)/(u.f + 1.0f); z = f*f; return f*(Lg0+z*(Lg1+z*(Lg2+z*(Lg3+z*Lg4)))) + dk*ln2_lo + dk*ln2_hi; #endif } // Calculate 10.0 * log10f(x) float vna_log10f_x_10(float x) { union {float f; int32_t i;} u = {x}; if (u.i <= 0) return -1/0.0f; // if <=0 return -inf #if 0 // Give up to ~0.027 error const float MULT = (10.0f * logf(2.0f) / logf(10.0f)); const int mx = (u.i >> 23) - 0x7f; u.i = (u.i&0x007FFFFF) + 0x3F800000; return (((-1.0f/3) * u.f + 2.0f) * u.f - (5.0f/3) + mx) * MULT; #elif 1 // Give up to ~2.377e-4 error union { int32_t i; float f; } mx = { (u.i & 0x007FFFFF) | 0x3f000000 }; // return u.i * (MULT / (1 << 23)) - (124.225784301758f * MULT) - (1.497851252556f * MULT) * mx.f - (1.725635766983f * MULT) / (0.352076232433f + mx.f); return u.i * 3.588558655063e-07 - 373.955116469345 - 4.509594876113 * mx.f - 5.197150890108 / (0.352256419296 + mx.f); #else // Give up to ~3.12e-6 error 0.0000038147f float f, z, mx; static const float // Stored as float ln2_hi = 3.010264109333, // 3.010264158249 ln2_lo = 0.000035784926, // 0.000035784928 Lg0 = 8.685889638065, // 20.0 / log(10.0) Lg1 = 2.895343684930, // 2.895343780518 Lg2 = 1.735529785085, // 1.735529780388 Lg3 = 1.257874186900, // 1.257874131203 Lg4 = 1.062503803849; // 1.062503814697 u.i += 0x3f800000 - 0x3f3504f3;// reduce x into [sqrt(2)/2, sqrt(2)] mx = (u.i>>23) - 0x7f; u.i = (u.i&0x007fffff) + 0x3f3504f3; f = (u.f - 1.0f)/(u.f + 1.0f); z = f*f; return f*(Lg0+z*(Lg1+z*(Lg2+z*(Lg3+z*Lg4)))) + mx*ln2_lo + mx*ln2_hi; #endif } //********************************************************************************** // atanf //********************************************************************************** // __ieee754_atanf float vna_atanf(float x) { static const float atanhi[] = { 4.6364760399e-01, // atan(0.5)hi 0x3eed6338 7.8539812565e-01, // atan(1.0)hi 0x3f490fda 9.8279368877e-01, // atan(1.5)hi 0x3f7b985e 1.5707962513e+00, // atan(inf)hi 0x3fc90fda }; static const float atanlo[] = { 5.0121582440e-09, // atan(0.5)lo 0x31ac3769 3.7748947079e-08, // atan(1.0)lo 0x33222168 3.4473217170e-08, // atan(1.5)lo 0x33140fb4 7.5497894159e-08, // atan(inf)lo 0x33a22168 }; static const float aT[] = { 3.3333328366e-01, -1.9999158382e-01, 1.4253635705e-01, -1.0648017377e-01, 6.1687607318e-02, }; float w,s1,s2,z; uint32_t ix,sign; int id; union {float f; uint32_t i;} u = {x}; ix = u.i; sign = ix>>31; ix &= 0x7fffffff; if (ix >= 0x4c800000) { /* if |x| >= 2**26 */ if (ix > 0x7f800000) return x; z = atanhi[3] + 0x1p-120f; return sign ? -z : z; } if (ix < 0x3ee00000) { /* |x| < 0.4375 */ if (ix < 0x39800000) { /* |x| < 2**-12 */ return x; } id = -1; } else { x = vna_fabsf(x); if (ix < 0x3f980000) { /* |x| < 1.1875 */ if (ix < 0x3f300000) { /* 7/16 <= |x| < 11/16 */ id = 0; x = (2.0f*x - 1.0f)/(2.0f + x); } else { /* 11/16 <= |x| < 19/16 */ id = 1; x = (x - 1.0f)/(x + 1.0f); } } else { if (ix < 0x401c0000) { /* |x| < 2.4375 */ id = 2; x = (x - 1.5f)/(1.0f + 1.5f*x); } else { /* 2.4375 <= |x| < 2**26 */ id = 3; x = -1.0f/x; } } } /* end of argument reduction */ z = x*x; w = z*z; /* break sum from i=0 to 10 aT[i]z**(i+1) into odd and even poly */ s1 = z*(aT[0]+w*(aT[2]+w*aT[4])); s2 = w*(aT[1]+w*aT[3]); if (id < 0) return x - x*(s1+s2); z = atanhi[id] - ((x*(s1+s2) - atanlo[id]) - x); return sign ? -z : z; } //********************************************************************************** // atan2f //********************************************************************************** #if 0 // __ieee754_atan2f float vna_atan2f(float y, float x) { static const float pi = 3.1415927410e+00; // 0x40490fdb static const float pi_lo =-8.7422776573e-08; // 0xb3bbbd2e float z; uint32_t m,ix,iy; union {float f; uint32_t i;} ux = {x}; union {float f; uint32_t i;} uy = {y}; ix = ux.i; iy = uy.i; if (ix == 0x3f800000) /* x=1.0 */ return vna_atanf(y); m = ((iy>>31)&1) | ((ix>>30)&2); /* 2*sign(x)+sign(y) */ ix &= 0x7fffffff; iy &= 0x7fffffff; /* when y = 0 */ if (iy == 0) { switch (m) { case 0: case 1: return y; // atan(+-0,+anything)=+-0 case 2: return pi; // atan(+0,-anything) = pi case 3: return -pi; // atan(-0,-anything) =-pi } } /* when x = 0 */ if (ix == 0) return m&1 ? -pi/2 : pi/2; /* when x is INF */ if (ix == 0x7f800000) { if (iy == 0x7f800000) { switch (m) { case 0: return pi/4; /* atan(+INF,+INF) */ case 1: return -pi/4; /* atan(-INF,+INF) */ case 2: return 3*pi/4; /*atan(+INF,-INF)*/ case 3: return -3*pi/4; /*atan(-INF,-INF)*/ } } else { switch (m) { case 0: return 0.0f; /* atan(+...,+INF) */ case 1: return -0.0f; /* atan(-...,+INF) */ case 2: return pi; /* atan(+...,-INF) */ case 3: return -pi; /* atan(-...,-INF) */ } } } /* |y/x| > 0x1p26 */ if (ix+(26<<23) < iy || iy == 0x7f800000) return m&1 ? -pi/2 : pi/2; /* z = atan(|y/x|) with correct underflow */ if ((m&2) && iy+(26<<23) < ix) /*|y/x| < 0x1p-26, x < 0 */ z = 0.0; else z = vna_atanf(vna_fabsf(y/x)); switch (m) { case 0: return z; /* atan(+,+) */ case 1: return -z; /* atan(-,+) */ case 2: return pi - (z-pi_lo); /* atan(+,-) */ default: /* case 3 */ return (z-pi_lo) - pi; /* atan(-,-) */ } } // Return atan(x, y) value in degree (-180 ... +180) float vna_atan2f_deg(float y, float x) { return vna_atan2f(y, x) * (180.0f / VNA_PI)); } #else // Polynomial approximation to atan2f float vna_atan2f (float y, float x) { union {float f; int32_t i;} ux = {x}; union {float f; int32_t i;} uy = {y}; if (ux.i == 0 && uy.i == 0) return 0.0f; float ax, ay, r, s; ax = vna_fabsf(x); ay = vna_fabsf(y); r = (ay < ax) ? ay / ax : ax / ay; s = r * r; // Polynomial approximation to atan(a) on [0,1] #if 0 // give 0.31 degree error r*= 0.970562748477141f - 0.189514164974601f * s; //r*= vna_fmaf(s, -0.189514164974601f, 0.970562748477141f); #elif 0 // give 0.04 degree error r*= 0.994949366116654f + s * (-0.287060635532652f + 0.078037176446441f * s); //r*= vna_fmaf(s, vna_fmaf(s, 0.078037176446441f, -0.287060635532652f), 0.994949366116654f); #else // give 0.005 degree error r*= 0.999133448222780f + s * (-0.320533292381664f + s * (0.144982490144465f + s * -0.038254464970299f)); //r*= vna_fmaf(s, vna_fmaf(s, vna_fmaf(s, -0.038254464970299f, 0.144982490144465f), -0.320533292381664f), 0.999133448222780f); #endif // Map to full circle if (ay > ax) r = VNA_PI/2.0f - r; if (ux.i < 0) r = VNA_PI - r; if (uy.i < 0) r = -r; return r; } // Return atan(x, y) value in degree (-180 ... +180) float vna_atan2f_deg(float y, float x) { union {float f; int32_t i;} ux = {x}; union {float f; int32_t i;} uy = {y}; if (ux.i == 0 && uy.i == 0) return 0.0f; float ax, ay, r, s; ax = vna_fabsf(x); ay = vna_fabsf(y); r = (ay < ax) ? ay / ax : ax / ay; s = r * r; // Polynomial approximation to atan(a) #if 0 // max ~0.283729 degree error r*= 55.714078993f + s * -10.997807686f; // r*= vna_fmaf(s, -10.997807686f, 55.714078993f); #elif 0 //max ~0.034870 degree error r*= 57.029809913f + s * (-16.540732225f + s * 4.545792223f); // r*= vna_fmaf(s, vna_fmaf(s, 4.545792223f, -16.540732225f), 57.029809913f); #elif 0 // max ~0.004662 degree error r*= 57.250736237f + s * (-18.401971817f + s * (8.380335808f + s * -2.233763933f)); // r*= vna_fmaf(s, vna_fmaf(s, vna_fmaf(s, -2.233763933f, 8.380335808f),-18.401971817f), 57.250736237f); #else // max ~0.000655 degree error r*= 57.288120755f + s * (-18.925070157f + s * (10.322367203f + s * (-4.879099474f + s * 1.194337053f))); // r*= vna_fmaf(s, vna_fmaf(s, vna_fmaf(s, vna_fmaf(s, 1.194337053f, -4.879099474f), 10.322367203f), -18.925070157f), 57.288120755f); #endif // Map to full circle (0-90, 90-180, etc.) if (ay > ax) r = 90.0f - r; if (ux.i < 0) r = 180.0f - r; if (uy.i < 0) r = -r; return r; } #endif //********************************************************************************** // Fast expf approximation //********************************************************************************** float vna_expf(float x) { union { float f; int32_t i; } v; x*= (float)(1<<23) / logf(2.0f); v.i = (int32_t)x + 0x3F800000; int32_t m = (v.i >> 7) & 0xFFFF; // copy mantissa #if 0 // cubic spline approximation, empirical values for small maximum relative error (8.34e-5): v.i += ((((((((1277*m) >> 14) + 14825)*m) >> 14) - 79749)*m) >> 11) - 626; #else // quartic spline approximation, empirical values for small maximum relative error (1e-5): v.i += (((((((((((903*m) >> 14) + 13521)*m) >> 18) + 15838)*m) >> 14) - 80482)*m) >> 11); #endif return v.f; }