From 368f5126f7038079571bb9874e8c587829c75bda Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Pawe=C5=82=20Spychalski?= Date: Fri, 6 Mar 2026 10:17:25 +0100 Subject: [PATCH 1/3] Use fast_powf instead of powf for better performance in PID calculations. --- src/main/common/maths.c | 65 +++++++++++++++++++++++++++++++++++++++++ src/main/common/maths.h | 1 + src/main/flight/pid.c | 4 +-- 3 files changed, 68 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 993634d902d..a6202efdb6f 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -105,6 +105,71 @@ float acos_approx(float x) } #endif +/** + * Fast power approximation for positive base values. + * Uses bit manipulation via the identity: x^y = 2^(y * log2(x)) + * Optimized for embedded systems - approximately 5-10x faster than powf(). + * + * Accuracy: ~1-3% error for typical ranges, sufficient for TPA calculations. + * Note: Only valid for base > 0. Returns 0 for invalid inputs. + */ +float fast_powf(float base, float exp) +{ + // Handle common special cases for maximum speed + if (exp == 0.0f) { + return 1.0f; + } + if (exp == 1.0f) { + return base; + } + if (base <= 0.0f) { + return 0.0f; // Invalid input + } + if (exp == 2.0f) { + return base * base; + } + if (exp == 0.5f) { + return fast_fsqrtf(base); + } + + // For general case, use bit manipulation approximation + // Based on: x^y = 2^(y * log2(x)) + // Using IEEE 754 floating point representation + union { + float f; + int32_t i; + } u; + + u.f = base; + // Extract and compute: log2(x) ≈ (mantissa bits - 127) + normalized_mantissa + // IEEE 754: float = 2^(exponent-127) * (1 + mantissa/2^23) + // log2(x) ≈ (exponent - 127) + (mantissa / 2^23) + + // Fast approximation: just use the exponent bits for log2 + // More accurate version includes mantissa contribution + int32_t exp_bits = (u.i >> 23) & 0xFF; + int32_t mant_bits = u.i & 0x7FFFFF; + + // log2(base) approximation with mantissa correction + float log2_base = (float)(exp_bits - 127) + (float)mant_bits / 8388608.0f; + + // Compute result exponent: y * log2(x) + float result_exp = exp * log2_base; + + // Convert back to float: 2^result_exp + int32_t result_exp_int = (int32_t)result_exp; + float result_exp_frac = result_exp - (float)result_exp_int; + + // Reconstruct float from exponent + u.i = (result_exp_int + 127) << 23; + + // Apply fractional part correction using polynomial approximation + // 2^x ≈ 1 + x*(0.69315 + x*(0.24023 + x*0.05550)) for x in [0,1] + float frac_mult = 1.0f + result_exp_frac * (0.69314718f + result_exp_frac * (0.24022650f + result_exp_frac * 0.05550410f)); + + return u.f * frac_mult; +} + int gcd(int num, int denom) { if (denom == 0) { diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 675ff56c2ca..9726baba00e 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -205,6 +205,7 @@ float bellCurve(const float x, const float curveWidth); float attenuation(const float input, const float width); float gaussian(const float x, const float mu, const float sigma); float fast_fsqrtf(const float value); +float fast_powf(float base, float exp); float calc_length_pythagorean_2D(const float firstElement, const float secondElement); float calc_length_pythagorean_3D(const float firstElement, const float secondElement, const float thirdElement); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0a2faa648ca..e65d72e9c42 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -445,7 +445,7 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate) static float calculateFixedWingAirspeedTPAFactor(void){ const float airspeed = constrainf(getAirspeedEstimate(), 100.0f, 20000.0f); // cm/s, clamped to 3.6-720 km/h const float referenceAirspeed = pidProfile()->fixedWingReferenceAirspeed; // in cm/s - float tpaFactor= powf(referenceAirspeed/airspeed, currentControlProfile->throttle.apa_pow/100.0f); + float tpaFactor= fast_powf(referenceAirspeed/airspeed, currentControlProfile->throttle.apa_pow/100.0f); tpaFactor= constrainf(tpaFactor, 0.3f, 2.0f); return tpaFactor; } @@ -460,7 +460,7 @@ static float calculateFixedWingAirspeedITermFactor(void){ return 1.0f; } - float iTermFactor = powf(referenceAirspeed/airspeed, (apa_pow/100.0f) - 1.0f); + float iTermFactor = fast_powf(referenceAirspeed/airspeed, (apa_pow/100.0f) - 1.0f); iTermFactor = constrainf(iTermFactor, 0.3f, 1.5f); return iTermFactor; } From d0926bde38bb544e9e12dcc50295ecf8354f963e Mon Sep 17 00:00:00 2001 From: Sensei Date: Sun, 17 May 2026 21:57:26 -0500 Subject: [PATCH 2/3] Fix exponent calculation in maths.cfast_powf: apply floorf() in case of negative Use floorf to calculate integer part of exponent. --- src/main/common/maths.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index c5214b430b5..ff79e1b3cc0 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -157,7 +157,7 @@ float fast_powf(float base, float exp) float result_exp = exp * log2_base; // Convert back to float: 2^result_exp - int32_t result_exp_int = (int32_t)result_exp; + int32_t result_exp_int = (int32_t)floorf(result_exp); float result_exp_frac = result_exp - (float)result_exp_int; // Reconstruct float from exponent @@ -681,4 +681,4 @@ void arm_mult_f32( } } -#endif \ No newline at end of file +#endif From 0b0404d92400b5b225159ffb74ab20c124fdd522 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Mon, 18 May 2026 03:02:54 +0000 Subject: [PATCH 3/3] Fix fast_powf: use floorf for integer decomposition; add unit tests - Use floorf() instead of (int32_t) cast when decomposing result_exp into integer and fractional parts. The (int32_t) cast truncates toward zero, so for negative result_exp (base<1, common in airspeed TPA scaling) the fractional part would be negative, outside the [0,1] domain of the 2^x polynomial approximation. - Add TestFastPowf unit test covering special cases, base<1 cases (key regression), and base>1 cases with appropriate error tolerances. Agent-Logs-Url: https://github.com/iNavFlight/inav/sessions/ac361c30-4841-46d4-8d06-d704c02a2f7c Co-authored-by: sensei-hacker <1971284+sensei-hacker@users.noreply.github.com> --- src/main/common/maths.c | 4 +++- src/test/unit/maths_unittest.cc | 30 ++++++++++++++++++++++++++++++ 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index c5214b430b5..09820c51c92 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -157,7 +157,9 @@ float fast_powf(float base, float exp) float result_exp = exp * log2_base; // Convert back to float: 2^result_exp - int32_t result_exp_int = (int32_t)result_exp; + // Use floorf to ensure fractional part is always in [0, 1) for the polynomial approximation. + // Simple (int32_t) cast truncates toward zero, giving a negative frac for negative result_exp. + int32_t result_exp_int = (int32_t)floorf(result_exp); float result_exp_frac = result_exp - (float)result_exp_int; // Reconstruct float from exponent diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 77779d366e2..cc4bc9c7e63 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -236,3 +236,33 @@ TEST(MathsUnittest, TestSensorScaleUnitTest) } */ #endif + +TEST(MathsUnittest, TestFastPowf) +{ + // Special cases handled exactly by short-circuit branches + EXPECT_FLOAT_EQ(fast_powf(2.0f, 0.0f), 1.0f); + EXPECT_FLOAT_EQ(fast_powf(2.0f, 1.0f), 2.0f); + EXPECT_FLOAT_EQ(fast_powf(3.0f, 2.0f), 9.0f); + EXPECT_NEAR(fast_powf(4.0f, 0.5f), 2.0f, 0.01f); + + // General approximation via bit-manipulation: typical error is ~5-10% + // base < 1 with non-trivial exponent - the key regression case for base<1 where + // result_exp is negative and fractional (floorf fix ensures frac stays in [0,1)) + EXPECT_NEAR(fast_powf(0.5f, 1.5f), powf(0.5f, 1.5f), powf(0.5f, 1.5f) * 0.10f); + EXPECT_NEAR(fast_powf(0.7f, 1.5f), powf(0.7f, 1.5f), powf(0.7f, 1.5f) * 0.10f); + EXPECT_NEAR(fast_powf(0.3f, 1.5f), powf(0.3f, 1.5f), powf(0.3f, 1.5f) * 0.10f); + + // base > 1 cases with non-trivial exponents + EXPECT_NEAR(fast_powf(2.0f, 1.5f), powf(2.0f, 1.5f), powf(2.0f, 1.5f) * 0.10f); + EXPECT_NEAR(fast_powf(10.0f, 1.5f), powf(10.0f, 1.5f), powf(10.0f, 1.5f) * 0.10f); + + // Short-circuit branches: exp==1.0f and exp==2.0f return exact values + EXPECT_FLOAT_EQ(fast_powf(0.015f, 1.0f), 0.015f); + EXPECT_FLOAT_EQ(fast_powf(60.0f, 2.0f), 3600.0f); + // Power-of-2 base with non-integer exponent: log2 is exact so result is close + EXPECT_NEAR(fast_powf(0.25f, 1.5f), powf(0.25f, 1.5f), powf(0.25f, 1.5f) * 0.01f); + + // Invalid input + EXPECT_FLOAT_EQ(fast_powf(0.0f, 2.0f), 0.0f); + EXPECT_FLOAT_EQ(fast_powf(-1.0f, 2.0f), 0.0f); +}