diff --git a/flight/libraries/math/mathmisc.h b/flight/libraries/math/mathmisc.h index 0fc95f963..ba9062a0d 100644 --- a/flight/libraries/math/mathmisc.h +++ b/flight/libraries/math/mathmisc.h @@ -120,4 +120,19 @@ static inline float y_on_curve(float x, const pointf points[], int num_points) return y_on_line(x, &points[end_point - 1], &points[end_point]); } +/** + * Ultrafast pow() aproximation needed for expo + * Based on Algorithm by Martin Ankerl + */ +static inline float fastPow(float a, float b) +{ + union { + double d; + int32_t x[2]; + } u = { (double)a }; + u.x[1] = (int32_t)(b * (u.x[1] - 1072632447) + 1072632447); + u.x[0] = 0; + return (float)u.d; +} + #endif /* MATHMISC_H */ diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index e2696d3c9..7808f9f02 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -29,6 +29,7 @@ */ #include "inc/manualcontrol.h" +#include #include #include #include @@ -44,7 +45,8 @@ static float applyExpo(float value, float expo); static float applyExpo(float value, float expo) { - float exp = powf(1.023293f, expo); + // note: fastPow makes a small error, therefore result needs to be bound + float exp = boundf(fastPow(1.023293f, expo), -10.0f, 10.0f); // magic number scales expo // so that @@ -53,9 +55,11 @@ static float applyExpo(float value, float expo) // expo=-100 yields value**(1/10) // (pow(10,1/100)~=1.023293) if (value > 0.0f) { - return powf(value, exp); + return boundf(fastPow(value, exp), 0.0f, 1.0f); + } else if (value < -0.0f) { + return boundf(-fastPow(-value, exp), -1.0f, 0.0f); } else { - return -powf(-value, exp); + return 0.0f; } }