perf(maths): backport BF #14790 O(1) sin/cos range reduction

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) {

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)
@@ -182,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;

src/main/common/maths.h

@@ -30,15 +30,15 @@
 #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
 #define M_PI_HALFf  3.14159265358979323846f/2
 #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);

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;
     }

src/main/fc/fc_rc.c

@@ -222,19 +222,17 @@ 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) {
         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];

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 {

src/test/unit/maths_unittest.cc

@@ -19,6 +19,7 @@
 #include <stdbool.h>
 
 #include <limits.h>
+#include <cfloat>
 
 #include <math.h>
 
@@ -202,17 +203,18 @@
     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);
         double libmResult = sinf(x);
         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 @@
     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 = 0.0f, cosCombined = 0.0f;
+        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 @@
     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)