From a3b57386c29545f07451555232ad102d14faf130 Mon Sep 17 00:00:00 2001 From: digitalentity Date: Tue, 7 Jul 2015 19:30:02 +1000 Subject: [PATCH] Ported fast trigonometry functions over from @Crashpilot1000 Harakiri code Fixed maths_unittest.cc to correctly handle float-point numbers --- src/main/common/maths.c | 50 +++++++++++++++++++++++++++++---- src/main/common/maths.h | 12 ++++++++ src/main/flight/imu.c | 12 ++++---- src/main/flight/navigation.c | 12 ++++---- src/main/mw.c | 4 +-- src/test/unit/maths_unittest.cc | 30 ++++++++++---------- 6 files changed, 85 insertions(+), 35 deletions(-) diff --git a/src/main/common/maths.c b/src/main/common/maths.c index cc95e9e44..e1eead8d6 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -21,6 +21,44 @@ #include "axis.h" #include "maths.h" +// 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 +#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY) +#if defined(EVEN_FASTER_TRIGONOMETRY) +#define sinApproxCoef3 -1.666568107e-1f +#define sinApproxCoef5 8.312366210e-3f +#define sinApproxCoef7 -1.849218155e-4f +#else +#define sinApproxCoef3 -1.666665710e-1f // Double: -1.666665709650470145824129400050267289858e-1 +#define sinApproxCoef5 8.333017292e-3f // Double: 8.333017291562218127986291618761571373087e-3 +#define sinApproxCoef7 -1.980661520e-4f // Double: -1.980661520135080504411629636078917643846e-4 +#define sinApproxCoef9 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; +#if defined(EVEN_FASTER_TRIGONOMETRY) + return x + x * x2 * (sinApproxCoef3 + x2 * (sinApproxCoef5 + x2 * sinApproxCoef7)); +#else + return x + x * x2 * (sinApproxCoef3 + x2 * (sinApproxCoef5 + x2 * (sinApproxCoef7 + x2 * sinApproxCoef9))); +#endif +} + +float cos_approx(float x) +{ + return sin_approx(x + (0.5f * M_PIf)); +} +#endif + int32_t applyDeadband(int32_t value, int32_t deadband) { if (ABS(value) < deadband) { @@ -111,12 +149,12 @@ void buildRotationMatrix(fp_angles_t *delta, float matrix[3][3]) float cosx, sinx, cosy, siny, cosz, sinz; float coszcosx, sinzcosx, coszsinx, sinzsinx; - cosx = cosf(delta->angles.roll); - sinx = sinf(delta->angles.roll); - cosy = cosf(delta->angles.pitch); - siny = sinf(delta->angles.pitch); - cosz = cosf(delta->angles.yaw); - sinz = sinf(delta->angles.yaw); + 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); coszcosx = cosz * cosx; sinzcosx = sinz * cosx; diff --git a/src/main/common/maths.h b/src/main/common/maths.h index aaa6a0d18..ce96a80e9 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -21,6 +21,10 @@ #define sq(x) ((x)*(x)) #endif +// Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations +#define FAST_TRIGONOMETRY // order 9 approximation +//#define EVEN_FASTER_TRIGONOMETRY // order 7 approximation + // Use floating point M_PI instead explicitly. #define M_PIf 3.14159265358979323846f @@ -83,3 +87,11 @@ int32_t quickMedianFilter3(int32_t * v); int32_t quickMedianFilter5(int32_t * v); int32_t quickMedianFilter7(int32_t * v); int32_t quickMedianFilter9(int32_t * v); + +#if defined(FAST_TRIGONOMETRY) || defined(EVEN_FASTER_TRIGONOMETRY) +float sin_approx(float x); +float cos_approx(float x); +#else +#define sin_approx(x) sinf(x) +#define cos_approx(x) cosf(x) +#endif \ No newline at end of file diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 468929c64..ff599d51e 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -86,7 +86,7 @@ void imuConfigure( void imuInit() { - smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); + smallAngle = lrintf(acc_1G * cos_approx(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; gyroScaleRad = gyro.scale * (M_PIf / 180.0f) * 0.000001f; } @@ -208,10 +208,10 @@ int16_t imuCalculateHeading(t_fp_vector *vec) { int16_t head; - float cosineRoll = cosf(anglerad[AI_ROLL]); - float sineRoll = sinf(anglerad[AI_ROLL]); - float cosinePitch = cosf(anglerad[AI_PITCH]); - float sinePitch = sinf(anglerad[AI_PITCH]); + float cosineRoll = cos_approx(anglerad[AI_ROLL]); + float sineRoll = sin_approx(anglerad[AI_ROLL]); + float cosinePitch = cos_approx(anglerad[AI_PITCH]); + float sinePitch = sin_approx(anglerad[AI_PITCH]); float Xh = vec->A[X] * cosinePitch + vec->A[Y] * sineRoll * sinePitch + vec->A[Z] * sinePitch * cosineRoll; float Yh = vec->A[Y] * cosineRoll - vec->A[Z] * sineRoll; //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero, @@ -326,5 +326,5 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) int angle = lrintf(acosf(cosZ) * throttleAngleScale); if (angle > 900) angle = 900; - return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PIf / 2.0f))); + return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); } diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 64fdc0591..e460a2dc3 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -413,7 +413,7 @@ void gpsUsePIDs(pidProfile_t *pidProfile) static void GPS_calc_longitude_scaling(int32_t lat) { float rads = (ABS((float)lat) / 10000000.0f) * 0.0174532925f; - GPS_scaleLonDown = cosf(rads); + GPS_scaleLonDown = cos_approx(rads); } //////////////////////////////////////////////////////////////////////////////////// @@ -559,8 +559,8 @@ static void GPS_calc_nav_rate(uint16_t max_speed) // nav_bearing includes crosstrack temp = (9000l - nav_bearing) * RADX100; - trig[GPS_X] = cosf(temp); - trig[GPS_Y] = sinf(temp); + trig[GPS_X] = cos_approx(temp); + trig[GPS_Y] = sin_approx(temp); for (axis = 0; axis < 2; axis++) { rate_error[axis] = (trig[axis] * max_speed) - actual_speed[axis]; @@ -583,7 +583,7 @@ static void GPS_update_crosstrack(void) { if (ABS(wrap_18000(target_bearing - original_target_bearing)) < 4500) { // If we are too far off or too close we don't do track following float temp = (target_bearing - original_target_bearing) * RADX100; - crosstrack_error = sinf(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line + crosstrack_error = sin_approx(temp) * (wp_distance * CROSSTRACK_GAIN); // Meters we are off track line nav_bearing = target_bearing + constrain(crosstrack_error, -3000, 3000); nav_bearing = wrap_36000(nav_bearing); } else { @@ -644,8 +644,8 @@ static int32_t wrap_36000(int32_t angle) void updateGpsStateForHomeAndHoldMode(void) { - float sin_yaw_y = sinf(heading * 0.0174532925f); - float cos_yaw_x = cosf(heading * 0.0174532925f); + float sin_yaw_y = sin_approx(heading * 0.0174532925f); + float cos_yaw_x = cos_approx(heading * 0.0174532925f); if (gpsProfile->nav_slew_rate) { nav_rated[LON] += constrain(wrap_18000(nav[LON] - nav_rated[LON]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); // TODO check this on uint8 nav_rated[LAT] += constrain(wrap_18000(nav[LAT] - nav_rated[LAT]), -gpsProfile->nav_slew_rate, gpsProfile->nav_slew_rate); diff --git a/src/main/mw.c b/src/main/mw.c index 7156330d5..4e2995e09 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -240,8 +240,8 @@ void annexCode(void) if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(heading - headFreeModeHold); - float cosDiff = cosf(radDiff); - float sinDiff = sinf(radDiff); + float cosDiff = cos_approx(radDiff); + float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; diff --git a/src/test/unit/maths_unittest.cc b/src/test/unit/maths_unittest.cc index 429f2396d..b1a93cc08 100644 --- a/src/test/unit/maths_unittest.cc +++ b/src/test/unit/maths_unittest.cc @@ -73,33 +73,33 @@ TEST(MathsUnittest, TestConstrainNegatives) TEST(MathsUnittest, TestConstrainf) { // Within bounds. - EXPECT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 2.0f), 1.0f); // Equal to bottom bound. - EXPECT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 2.0f), 1.0f); // Equal to top bound. - EXPECT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(1.0f, 0.0f, 1.0f), 1.0f); // Equal to both bottom and top bound. - EXPECT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(1.0f, 1.0f, 1.0f), 1.0f); // Above top bound. - EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); // Below bottom bound. - EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); // Above bouth bounds. - EXPECT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(2.0f, 0.0f, 1.0f), 1.0f); // Below bouth bounds. - EXPECT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); + EXPECT_FLOAT_EQ(constrainf(0, 1.0f, 2.0f), 1.0f); } TEST(MathsUnittest, TestDegreesToRadians) { - EXPECT_EQ(degreesToRadians(0), 0.0f); - EXPECT_EQ(degreesToRadians(90), 0.5f * M_PIf); - EXPECT_EQ(degreesToRadians(180), M_PIf); - EXPECT_EQ(degreesToRadians(-180), - M_PIf); + EXPECT_FLOAT_EQ(degreesToRadians(0), 0.0f); + EXPECT_FLOAT_EQ(degreesToRadians(90), 0.5f * M_PIf); + EXPECT_FLOAT_EQ(degreesToRadians(180), M_PIf); + EXPECT_FLOAT_EQ(degreesToRadians(-180), - M_PIf); } TEST(MathsUnittest, TestApplyDeadband) @@ -118,9 +118,9 @@ TEST(MathsUnittest, TestApplyDeadband) void expectVectorsAreEqual(struct fp_vector *a, struct fp_vector *b) { - EXPECT_EQ(a->X, b->X); - EXPECT_EQ(a->Y, b->Y); - EXPECT_EQ(a->Z, b->Z); + EXPECT_FLOAT_EQ(a->X, b->X); + EXPECT_FLOAT_EQ(a->Y, b->Y); + EXPECT_FLOAT_EQ(a->Z, b->Z); } TEST(MathsUnittest, TestRotateVectorWithNoAngle)