From fc024cdacc88a4f68c1a22f09995cd5e30a30388 Mon Sep 17 00:00:00 2001
From: Mathias Rasmussen <mathiasvr@gmail.com>
Date: Fri, 3 Dec 2021 06:38:40 +0100
Subject: [PATCH] Optimize rescueAttainPosition()

- Fix previousSpeedError type to avoid unnecessary float conversion
- Fix unnecessary static scalingRate variable for better optimisation,
  and reorganise usage for clarity
---
 src/main/flight/gps_rescue.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index 30ce7e9073..89806365da 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -282,12 +282,10 @@ static void setBearing(int16_t desiredHeading)
 static void rescueAttainPosition()
 {
     // Speed and altitude controller internal variables
-    static float previousSpeedError = 0;
+    static int16_t previousSpeedError = 0;
     static int16_t speedIntegral = 0;
-    int zVelocityError;
     static int previousZVelocityError = 0;
     static float zVelocityIntegral = 0;
-    static float scalingRate = 0;
     static int16_t altitudeAdjustment = 0;
 
     if (rescueState.phase == RESCUE_INITIALIZE) {
@@ -332,12 +330,14 @@ static void rescueAttainPosition()
     const int16_t altitudeError = rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm;
 
     // P component
+    float scalingRate;
     if (ABS(altitudeError) > 0 && ABS(altitudeError) < GPS_RESCUE_ZVELOCITY_THRESHOLD) {
         scalingRate = (float)altitudeError / GPS_RESCUE_ZVELOCITY_THRESHOLD;
     } else {
         scalingRate = 1;
     }
 
+    int zVelocityError;
     if (altitudeError > 0) {
         zVelocityError = gpsRescueConfig()->ascendRate * scalingRate - rescueState.sensor.zVelocity;
     } else if (altitudeError < 0) {