diff --git a/src/main/common/maths.h b/src/main/common/maths.h index f9f5456943..513a36c182 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -26,6 +26,7 @@ #define sq(x) ((x)*(x)) #endif #define power3(x) ((x)*(x)*(x)) +#define power5(x) ((x)*(x)*(x)*(x)*(x)) // Undefine this for use libc sinf/cosf. Keep this defined to use fast sin/cos approximations #define FAST_MATH // order 9 approximation diff --git a/src/main/fc/rc.c b/src/main/fc/rc.c index 6cbaed5191..3589de9c21 100644 --- a/src/main/fc/rc.c +++ b/src/main/fc/rc.c @@ -208,7 +208,7 @@ float applyKissRates(const int axis, float rcCommandf, const float rcCommandfAbs float applyActualRates(const int axis, float rcCommandf, const float rcCommandfAbs) { float expof = currentControlRateProfile->rcExpo[axis] / 100.0f; - expof = rcCommandfAbs * (powf(rcCommandf, 5) * expof + rcCommandf * (1 - expof)); + expof = rcCommandfAbs * (power5(rcCommandf) * expof + rcCommandf * (1 - expof)); const float centerSensitivity = currentControlRateProfile->rcRates[axis] * 10.0f; const float stickMovement = MAX(0, currentControlRateProfile->rates[axis] * 10.0f - centerSensitivity); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f7f8653b70..517c15cf1d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -338,7 +338,7 @@ float pidCompensateThrustLinearization(float throttle) if (pidRuntime.thrustLinearization != 0.0f) { // for whoops where a lot of TL is needed, allow more throttle boost const float throttleReversed = (1.0f - throttle); - throttle /= 1.0f + pidRuntime.throttleCompensateAmount * powf(throttleReversed, 2); + throttle /= 1.0f + pidRuntime.throttleCompensateAmount * sq(throttleReversed); } return throttle; } @@ -348,7 +348,7 @@ float pidApplyThrustLinearization(float motorOutput) if (pidRuntime.thrustLinearization != 0.0f) { if (motorOutput > 0.0f) { const float motorOutputReversed = (1.0f - motorOutput); - motorOutput *= 1.0f + powf(motorOutputReversed, 2) * pidRuntime.thrustLinearization; + motorOutput *= 1.0f + sq(motorOutputReversed) * pidRuntime.thrustLinearization; } } return motorOutput; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index f8d7de86ac..a88e29c1e4 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -393,7 +393,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) #ifdef USE_THRUST_LINEARIZATION pidRuntime.thrustLinearization = pidProfile->thrustLinearization / 100.0f; - pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * powf(pidRuntime.thrustLinearization, 2); + pidRuntime.throttleCompensateAmount = pidRuntime.thrustLinearization - 0.5f * sq(pidRuntime.thrustLinearization); #endif #if defined(USE_D_MIN) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 208fb73fa9..1d05d79a4f 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -1787,7 +1787,7 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) int32_t dir; GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[GPS_LATITUDE], &lastCoord[GPS_LONGITUDE], &dist, &dir); if (gpsConfig()->gps_use_3d_speed) { - dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f)); + dist = sqrtf(sq(gpsSol.llh.altCm - lastAlt) + sq(dist)); } GPS_distanceFlownInCm += dist; }