From 1f9c828f2a5b1ae02f5e5d6540edfae6cfc7281a Mon Sep 17 00:00:00 2001
From: Michael Keller <github@ike.ch>
Date: Wed, 24 Jan 2018 12:27:07 +1300
Subject: [PATCH] Fixed rates / interpolation bug.

---
 src/main/fc/fc_rc.c | 25 +++++++++++--------------
 1 file changed, 11 insertions(+), 14 deletions(-)

diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c
index 3b2e69ae2..df91a84bc 100644
--- a/src/main/fc/fc_rc.c
+++ b/src/main/fc/fc_rc.c
@@ -192,8 +192,7 @@ void processRcCommand(void)
 
     const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
     uint16_t rxRefreshRate;
-    bool readyToCalculateRate = false;
-    uint8_t readyToCalculateRateAxisCnt = 0;
+    uint8_t updatedChannel = 0;
 
     if (rxConfig()->rcInterpolation) {
          // Set RC refresh rate for sampling and channels to filter
@@ -213,7 +212,7 @@ void processRcCommand(void)
         if (isRXDataNew && rxRefreshRate > 0) {
             rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
 
-            for (int channel=ROLL; channel < interpolationChannels; channel++) {
+            for (int channel = ROLL; channel < interpolationChannels; channel++) {
                 rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
             }
 
@@ -229,27 +228,22 @@ void processRcCommand(void)
 
         // Interpolate steps of rcCommand
         if (rcInterpolationStepCount > 0) {
-            for (int channel=ROLL; channel < interpolationChannels; channel++) {
-                rcCommandInterp[channel] += rcStepSize[channel];
-                rcCommand[channel] = rcCommandInterp[channel];
-                readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
+            for (updatedChannel = ROLL; updatedChannel < interpolationChannels; updatedChannel++) {
+                rcCommandInterp[updatedChannel] += rcStepSize[updatedChannel];
+                rcCommand[updatedChannel] = rcCommandInterp[updatedChannel];
             }
-            readyToCalculateRate = true;
         }
     } else {
         rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
     }
 
-    if (readyToCalculateRate || isRXDataNew) {
-        if (isRXDataNew) {
-            readyToCalculateRateAxisCnt = FD_YAW;
-        }
-
+    if (isRXDataNew || updatedChannel) {
+        const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
 #if defined(SITL)
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
 #endif
-        for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++) {
+        for (int axis = FD_ROLL; axis <= maxUpdatedAxis; axis++) {
 #if defined(SITL)
 #pragma GCC diagnostic pop
 #endif
@@ -260,11 +254,14 @@ void processRcCommand(void)
             debug[2] = rcInterpolationStepCount;
             debug[3] = setpointRate[0];
         }
+
         // Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
         if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE)) {
             scaleRcCommandToFpvCamAngle();
         }
+    }
 
+    if (isRXDataNew) {
         isRXDataNew = false;
     }
 }