From 1b95e3c20f155a7f4344b2896bfcf26f0664f30c Mon Sep 17 00:00:00 2001
From: Mathias Rasmussen <mathiasvr@gmail.com>
Date: Tue, 14 Dec 2021 11:53:09 +0100
Subject: [PATCH] Use macros for power with integer exponents

---
 src/main/common/maths.h    | 1 +
 src/main/fc/rc.c           | 2 +-
 src/main/flight/pid.c      | 4 ++--
 src/main/flight/pid_init.c | 2 +-
 src/main/io/gps.c          | 2 +-
 5 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index e0a655c04c..234262cdb1 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 45b010bb59..50732afd69 100644
--- a/src/main/fc/rc.c
+++ b/src/main/fc/rc.c
@@ -213,7 +213,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 a74641d05e..30617b1620 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -332,7 +332,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;
 }
@@ -342,7 +342,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 33ab676425..e23151bcf6 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -1511,7 +1511,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;
             }