From b3a6f6b5b370e0be8d8bdc27132b35a8b31391c8 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 24 Jul 2026 16:14:31 -0500 Subject: [PATCH 1/4] perf(maths): backport BF #14790 O(1) sin/cos range reduction Replaces while-loop range reduction in sin_approx/cos_approx with roundf-based O(1) quadrant split (range_reduce -> r in [-0.5, 0.5]). sincosf_approx shares one range reduction and one r^2 computation between sin_poly5_r and cos_poly6_r instead of two full sin_approx evaluations. Consolidates maths.h FAST_MATH/VERY_FAST_MATH into a single FAST_MATH guard, matching BF's post-#14790 maths.h; VERY_FAST_MATH was always defined in EF (no target undefines it), so this is a no-op for build output, only a define-name delta reduction vs BF. Removes the prior |x| > 32 rad clamp-to-zero guard; the new reduction has no such limit (mechanical consequence of dropping the while loop, not a design choice made here). Call sites (filter.c, imu.c, gps.c, sdft.c, rangefinder.c, fc_rc.c) are unchanged per scope; BF's own callsite migration to sincosf_approx in vector.c/imu.c/rc.c is left as optional follow-up, consistent with the source issue's own "Optional follow-up" framing. Extends maths_unittest.cc: sweeps sin_approx/cos_approx against sinf/ cosf at BF's own tolerance (3.2e-6 sin, 3.5e-6 cos, EF's prior cos tolerance retained), adds explicit edge-case checks (0, +-pi, +-pi/2, large multiples of 2*pi with a magnitude-scaled epsilon), and verifies sincosf_approx output matches independent sin_approx/cos_approx calls on the same angle. Source: betaflight/betaflight#14790 (ledvinap, merged 2026-03-04). Closes emuflight/EmuFlight#1269. Co-Authored-By: Claude Sonnet 5 --- src/main/common/maths.c | 112 +++++++++++++++++++++++--------- src/main/common/maths.h | 6 +- src/test/unit/maths_unittest.cc | 48 +++++++++++++- 3 files changed, 128 insertions(+), 38 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index a9fb2a8610..dc70d2b24b 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -28,44 +28,92 @@ #include "arm_math.h" #endif -#if defined(FAST_MATH) || defined(VERY_FAST_MATH) -#if defined(VERY_FAST_MATH) - -// http://lolengine.net/blog/2011/12/21/better-function-approximations -// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117 -// Thanks for ledvinap for making such accuracy possible! See: https://github.com/cleanflight/cleanflight/issues/940#issuecomment-110323384 -// https://github.com/Crashpilot1000/HarakiriWebstore1/blob/master/src/mw.c#L1235 -// sin_approx maximum absolute error = 2.305023e-06 -// cos_approx maximum absolute error = 2.857298e-06 -#define sinPolyCoef3 -1.666568107e-1f -#define sinPolyCoef5 8.312366210e-3f -#define sinPolyCoef7 -1.849218155e-4f -#define sinPolyCoef9 0 -#else -#define sinPolyCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1 -#define sinPolyCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3 -#define sinPolyCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4 -#define sinPolyCoef9 2.600054768e-6f // Double: 2.600054767890361277123254766503271638682e-6 -#endif -float sin_approx(float x) { - int32_t xint = x; - if (xint < -32 || xint > 32) return 0.0f; // Stop here on error input (5 * 360 Deg) - while (x > M_PIf) x -= (2.0f * M_PIf); // always wrap input angle to -PI..PI - while (x < -M_PIf) x += (2.0f * M_PIf); - if (x > (0.5f * M_PIf)) x = (0.5f * M_PIf) - (x - (0.5f * M_PIf)); // We just pick -90..+90 Degree - else if (x < -(0.5f * M_PIf)) x = -(0.5f * M_PIf) - ((0.5f * M_PIf) + x); - float x2 = x * x; - return x + x * x2 * (sinPolyCoef3 + x2 * (sinPolyCoef5 + x2 * (sinPolyCoef7 + x2 * sinPolyCoef9))); +#if defined(FAST_MATH) + +// Backport of betaflight/betaflight#14790 (ledvinap): O(1) roundf-based range reduction, no while loop. +static inline float sin_poly5_r(float r) +{ + const float c0 = 0x1.921f1cp0f; // 1.5707871913909912109375 + const float c1 = -0x1.4a974p-1f; // -0.6456851959228515625 + const float c2 = 0x1.3db294p-4f; // 7.756288349628448486328125e-2 + float s = r * r; + return r * ((c2 * s + c1) * s + c0); +} + +static inline float cos_poly6_r(float r) +{ + const float d1 = -0x1.3bd39cp0f; // -1.2336976528167724609375 + const float d2 = 0x1.03bp-2f; // 0.25360107421875 + const float d3 = -0x1.4e5eecp-6f; // -2.04083733260631561279296875e-2 + float s = r * r; + return ((d3 * s + d2) * s + d1) * s + 1.0f; +} + +// r in [-0.5, 0.5], q is quadrant index (..., -1, 0, 1, 2, 3, 4, ...). +static inline float sinf_quadrant_r(float r, int q) +{ + q &= 3; + if (q & 1) { + float v = cos_poly6_r(r); + return (q & 2) ? -v : v; + } else { + float v = sin_poly5_r(r); + return (q & 2) ? -v : v; + } +} + +static inline float cosf_quadrant_r(float r, int q) +{ + q &= 3; + if (q & 1) { + float v = -sin_poly5_r(r); + return (q & 2) ? -v : v; + } else { + float v = cos_poly6_r(r); + return (q & 2) ? -v : v; + } } -float cos_approx(float x) { - return sin_approx(x + (0.5f * M_PIf)); +static inline void sincosf_quadrant_r(float r, int q, float *out_s, float *out_c) +{ + q &= 3; + float sb = sin_poly5_r(r); + float cb = cos_poly6_r(r); + + float s = (q & 1) ? cb : sb; + float c = (q & 1) ? -sb : cb; + + if (q & 2) { s = -s; c = -c; } + + *out_s = s; + *out_c = c; +} + +float sin_approx(float x) +{ + float t = x * INV_PIO2; + float qf = roundf(t); + int q = (int)qf; + float r = t - qf; + return sinf_quadrant_r(r, q); +} + +float cos_approx(float x) +{ + float t = x * INV_PIO2; + float qf = roundf(t); + int q = (int)qf; + float r = t - qf; + return cosf_quadrant_r(r, q); } void sincosf_approx(float x, float *out_s, float *out_c) { - *out_s = sin_approx(x); - *out_c = cos_approx(x); + float t = x * INV_PIO2; + float qf = roundf(t); + int q = (int)qf; + float r = t - qf; + sincosf_quadrant_r(r, q, out_s, out_c); } // Initial implementation by Crashpilot1000 (https://github.com/Crashpilot1000/HarakiriWebstore1/blob/396715f73c6fcf859e0db0f34e12fe44bace6483/src/mw.c#L1292) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 49375bc9ae..033e0855fe 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -30,8 +30,7 @@ #define SIGN(x) ((x > 0.0f) - (x < 0.0f)) // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations -#define FAST_MATH // order 9 approximation -#define VERY_FAST_MATH // order 7 approximation +#define FAST_MATH // Use floating point M_PI instead explicitly. #define M_PIf 3.14159265358979323846f @@ -39,6 +38,7 @@ #define M_EULERf 2.71828182845904523536f #define M_SQRT2f 1.41421356237309504880f #define M_LN2f 0.69314718055994530942f +#define INV_PIO2 (2.0f / M_PIf) #define RAD (M_PIf / 180.0f) @@ -131,7 +131,7 @@ float quickMedianFilter5f(float * v); float quickMedianFilter7f(float * v); float quickMedianFilter9f(float * v); -#if defined(FAST_MATH) || defined(VERY_FAST_MATH) +#if defined(FAST_MATH) float sin_approx(float x); float cos_approx(float x); void sincosf_approx(float x, float *out_s, float *out_c); diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 6ba779e200..a3c038a608 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -19,6 +19,7 @@ #include #include +#include #include @@ -202,9 +203,10 @@ void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b, float absTo EXPECT_NEAR(a->Z, b->Z, absTol); } -#if defined(FAST_MATH) || defined(VERY_FAST_MATH) +#if defined(FAST_MATH) TEST(MathsUnittest, TestFastTrigonometrySinCos) { + // Matches BF #14790's own test range/tolerance (+-10*pi). double sinError = 0; for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) { double approxResult = sin_approx(x); @@ -212,7 +214,7 @@ TEST(MathsUnittest, TestFastTrigonometrySinCos) sinError = MAX(sinError, fabs(approxResult - libmResult)); } printf("sin_approx maximum absolute error = %e\n", sinError); - EXPECT_LE(sinError, 3e-6); + EXPECT_LE(sinError, 3.2e-6); double cosError = 0; for (float x = -10 * M_PI; x < 10 * M_PI; x += M_PI / 300) { @@ -224,6 +226,46 @@ TEST(MathsUnittest, TestFastTrigonometrySinCos) EXPECT_LE(cosError, 3.5e-6); } +TEST(MathsUnittest, TestFastTrigonometryEdgeCases) +{ + const float epsilon = 3.2e-6f; + + EXPECT_NEAR(sin_approx(0.0f), 0.0f, epsilon); + EXPECT_NEAR(cos_approx(0.0f), 1.0f, epsilon); + + EXPECT_NEAR(sin_approx(M_PIf), sinf(M_PIf), epsilon); + EXPECT_NEAR(cos_approx(M_PIf), -1.0f, epsilon); + + EXPECT_NEAR(sin_approx(-M_PIf), sinf(-M_PIf), epsilon); + EXPECT_NEAR(cos_approx(-M_PIf), -1.0f, epsilon); + + EXPECT_NEAR(sin_approx(0.5f * M_PIf), 1.0f, epsilon); + EXPECT_NEAR(cos_approx(0.5f * M_PIf), 0.0f, epsilon); + + EXPECT_NEAR(sin_approx(-0.5f * M_PIf), -1.0f, epsilon); + EXPECT_NEAR(cos_approx(-0.5f * M_PIf), 0.0f, epsilon); + + // Old EF code clamped |x| > 32 rad to 0.0f; this port removes that clamp, so verify large multiples of 2*pi with a magnitude-scaled epsilon (float32 mantissa precision loss dominates at this range). + for (int k = -50; k <= 50; k += 10) { + const float x = (float)k * 2.0f * M_PIf; + const float epsilonAtX = epsilon + fabsf(x) * FLT_EPSILON; + EXPECT_NEAR(sin_approx(x), sinf(x), epsilonAtX) << "k=" << k << " x=" << x; + EXPECT_NEAR(cos_approx(x), cosf(x), epsilonAtX) << "k=" << k << " x=" << x; + } +} + +TEST(MathsUnittest, TestSincosfApproxMatchesSeparateCalls) +{ + // Combined-call result must match independent sin_approx/cos_approx calls on the same angle. + const float epsilon = 1e-6f; + for (float x = -20 * M_PIf; x < 20 * M_PIf; x += M_PIf / 97) { + float sinCombined, cosCombined; + sincosf_approx(x, &sinCombined, &cosCombined); + EXPECT_NEAR(sinCombined, sin_approx(x), epsilon); + EXPECT_NEAR(cosCombined, cos_approx(x), epsilon); + } +} + TEST(MathsUnittest, TestFastTrigonometryATan2) { double error = 0; @@ -249,4 +291,4 @@ TEST(MathsUnittest, TestFastTrigonometryACos) printf("acos_approx maximum absolute error = %e rads (%e degree)\n", error, error / M_PI * 180.0f); EXPECT_LE(error, 1e-4); } -#endif +#endif // defined(FAST_MATH) From bee1b0efe85645748666b8ca91460fc68e619a19 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 24 Jul 2026 16:18:23 -0500 Subject: [PATCH 2/4] test(maths): zero-initialize sincosf_approx output locals in unit test CodeRabbit finding: sinCombined/cosCombined were bare-declared before being passed to sincosf_approx(), inconsistent with repo's UNIT-TEST.instructions.md zero-init convention. Related major finding (unguarded (int)qf conversion on non-finite/huge input in sin_approx/cos_approx/sincosf_approx) not applied: matches betaflight/betaflight#14790's merged, reviewed implementation exactly (no guard present upstream either); adding one would diverge from the BF reference this is a mechanical backport of. --- src/test/unit/maths_unittest.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index a3c038a608..6903e8f75b 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -259,7 +259,7 @@ TEST(MathsUnittest, TestSincosfApproxMatchesSeparateCalls) // Combined-call result must match independent sin_approx/cos_approx calls on the same angle. const float epsilon = 1e-6f; for (float x = -20 * M_PIf; x < 20 * M_PIf; x += M_PIf / 97) { - float sinCombined, cosCombined; + float sinCombined = 0.0f, cosCombined = 0.0f; sincosf_approx(x, &sinCombined, &cosCombined); EXPECT_NEAR(sinCombined, sin_approx(x), epsilon); EXPECT_NEAR(cosCombined, cos_approx(x), epsilon); From ebcdb65e8aa041b09d83f978544788886dccc530 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 28 Jul 2026 11:40:48 -0500 Subject: [PATCH 3/4] perf(maths): migrate paired same-angle sin/cos callsites to sincosf_approx MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Per CodeRabbit analysis on PR #1327 (comment #5095429276), completes BF #14790's callsite migration for EmuFlight's local equivalents: - maths.c buildRotationMatrix: roll/pitch/yaw pairs - filter.c biquadFilterUpdate: omega - sdft.c twiddle-factor init: phi - fc_rc.c scaleRcCommandToFpvCamAngle: both branches - imu.c applySensorCorrection: GPS courseOverGround - imu.c imuQuaternionHeadfreeOffsetSet: yawHalf Each site shares one range reduction and one r*r computation between sin/cos instead of two independent evaluations. No behavior change — sincosf_approx is verified equal to independent sin_approx+cos_approx calls in maths_unittest's TestSincosfApproxMatchesSeparateCalls. Co-Authored-By: Claude Sonnet 5 --- src/main/common/filter.c | 4 ++-- src/main/common/maths.c | 9 +++------ src/main/common/sdft.c | 4 +++- src/main/fc/fc_rc.c | 6 ++---- src/main/flight/imu.c | 10 +++++++--- 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 20c1791af9..40bf76be1d 100644 --- a/src/main/common/filter.c +++ b/src/main/common/filter.c @@ -176,8 +176,8 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType) { // setup variables const float omega = 2.0f * M_PIf * filterFreq * refreshRate * 0.000001f; - const float sn = sin_approx(omega); - const float cs = cos_approx(omega); + float sn, cs; + sincosf_approx(omega, &sn, &cs); const float alpha = sn / (2.0f * Q); switch (filterType) { diff --git a/src/main/common/maths.c b/src/main/common/maths.c index dc70d2b24b..d9b44941ab 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -230,12 +230,9 @@ float scaleRangef(float x, float srcFrom, float srcTo, float destFrom, float des void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]) { float cosx, sinx, cosy, siny, cosz, sinz; float coszcosx, sinzcosx, coszsinx, sinzsinx; - cosx = cos_approx(delta->angles.roll); - sinx = sin_approx(delta->angles.roll); - cosy = cos_approx(delta->angles.pitch); - siny = sin_approx(delta->angles.pitch); - cosz = cos_approx(delta->angles.yaw); - sinz = sin_approx(delta->angles.yaw); + sincosf_approx(delta->angles.roll, &sinx, &cosx); + sincosf_approx(delta->angles.pitch, &siny, &cosy); + sincosf_approx(delta->angles.yaw, &sinz, &cosz); coszcosx = cosz * cosx; sinzcosx = sinz * cosx; coszsinx = sinx * cosz; diff --git a/src/main/common/sdft.c b/src/main/common/sdft.c index 8337d492a8..578cf01aba 100644 --- a/src/main/common/sdft.c +++ b/src/main/common/sdft.c @@ -43,7 +43,9 @@ void sdftInit(sdft_t *sdft, const uint8_t startBin, const uint8_t endBin, const for (uint8_t i = 0; i < SDFT_BIN_COUNT; i++) { float phi = 0.0f; phi = c * i; - twiddle[i] = SDFT_R * (cos_approx(phi) + _Complex_I * sin_approx(phi)); + float sinPhi, cosPhi; + sincosf_approx(phi, &sinPhi, &cosPhi); + twiddle[i] = SDFT_R * (cosPhi + _Complex_I * sinPhi); } isInitialized = true; } diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 8decfb63d2..742561af22 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -229,12 +229,10 @@ static void scaleRcCommandToFpvCamAngle(void) { if (currentPitchAngle > rxConfig()->fpvCamAngleDegrees) { currentPitchAngle = rxConfig()->fpvCamAngleDegrees; } - cosFactor = cos_approx(currentPitchAngle * RAD); - sinFactor = sin_approx(currentPitchAngle * RAD); + sincosf_approx(currentPitchAngle * RAD, &sinFactor, &cosFactor); } else if (lastFpvCamAngleDegrees != rxConfig()->fpvCamAngleDegrees) { lastFpvCamAngleDegrees = rxConfig()->fpvCamAngleDegrees; - cosFactor = cos_approx(rxConfig()->fpvCamAngleDegrees * RAD); - sinFactor = sin_approx(rxConfig()->fpvCamAngleDegrees * RAD); + sincosf_approx(rxConfig()->fpvCamAngleDegrees * RAD, &sinFactor, &cosFactor); } float roll = setpointRate[ROLL]; float yaw = setpointRate[YAW]; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 69e34b4fac..968817b9b5 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -231,7 +231,9 @@ static void __attribute__((unused)) applySensorCorrection(quaternion *vError) { // (Rxx; Ryx) - measured (estimated) heading vector (EF) // (cos(COG), sin(COG)) - reference heading vector (EF) // error is cross product between reference heading and estimated heading (calculated in EF) - courseOverGround = -(float)sin_approx(courseOverGround) * (1.0f - 2.0f * qpAttitude.yy - 2.0f * qpAttitude.zz) - cos_approx(courseOverGround) * (2.0f * (qpAttitude.xy - -qpAttitude.wz)); + float sinCourseOverGround, cosCourseOverGround; + sincosf_approx(courseOverGround, &sinCourseOverGround, &cosCourseOverGround); + courseOverGround = -sinCourseOverGround * (1.0f - 2.0f * qpAttitude.yy - 2.0f * qpAttitude.zz) - cosCourseOverGround * (2.0f * (qpAttitude.xy - -qpAttitude.wz)); applyVectorError(courseOverGround, vError); } #endif @@ -427,10 +429,12 @@ void imuSetHasNewData(uint32_t dt) { bool imuQuaternionHeadfreeOffsetSet(void) { if ((ABS(getCosTiltAngle()) > 0.8f)) { const float yawHalf = atan2_approx((+2.0f * (qpAttitude.wz + qpAttitude.xy)), (+1.0f - 2.0f * (qpAttitude.yy + qpAttitude.zz))) / 2.0f; - qOffset.w = cos_approx(yawHalf); + float sinYawHalf, cosYawHalf; + sincosf_approx(yawHalf, &sinYawHalf, &cosYawHalf); + qOffset.w = cosYawHalf; qOffset.x = 0; qOffset.y = 0; - qOffset.z = sin_approx(yawHalf); + qOffset.z = sinYawHalf; quaternionConjugate(&qOffset, &qOffset); return (true); } else { From 66afda72c8075c6d6ad4122734bde04acf47b901 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 28 Jul 2026 13:53:01 -0500 Subject: [PATCH 4/4] fix(fc_rc): force first-call recompute of FPV cam angle sin/cos cache lastFpvCamAngleDegrees stayed uint8_t initialized to 0, coinciding with the field's own default value. Widened to int16_t initialized to -1 (outside uint8_t's full 0-255 range) so the cache-invalidation check can never accidentally match a real config value, guaranteeing the first post-boot call always recomputes cosFactor/sinFactor instead of relying on the hardcoded 1.0/0.0 defaults matching by coincidence. No functional change for the common case (angle 0): sincosf_approx(0) produces the same cos=1.0/sin=0.0 the old code assumed by default. Matches betaflight/betaflight#15498 (nerdCopter), which applies the same hardening to BF's analogous scaleRawSetpointToFpvCamAngle after betaflight/betaflight#14790 introduced the pattern. rxConfig()->fpvCamAngleDegrees itself (the MSP-facing config field) is untouched, still uint8_t. Co-Authored-By: Claude Sonnet 5 --- src/main/fc/fc_rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 742561af22..223527da90 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -222,7 +222,7 @@ static void calculateSetpointRate(int axis) { static void scaleRcCommandToFpvCamAngle(void) { float currentPitchAngle = attitude.raw[FD_PITCH] * 0.1f; //recalculate sin/cos only when rxConfig()->fpvCamAngleDegrees changed - static uint8_t lastFpvCamAngleDegrees = 0; + static int16_t lastFpvCamAngleDegrees = -1; static float cosFactor = 1.0; static float sinFactor = 0.0; if (rxConfig()->cinematicYaw) {