From 89a1aa7cc64671461e959cab84dfd4cfc42c8e6a Mon Sep 17 00:00:00 2001
From: Bruce Luckcuck <github@etracer.net>
Date: Sun, 27 May 2018 08:16:13 -0400
Subject: [PATCH] Fix yaw reset and throttle angle correction to be before rc
 interpolation In #5905 the ordering of rc interpolation processing was moved
 out of subTaskMainSubprocesses() and into a separate subTaskRcCommand()
 process that ran earlier in the PID loop. Previously the logic to handle
 stick arming yaw reset was immediately before the rc interpolation logic and
 after the previous change it ran later in the process and after rc
 interpolation.

This fix moves the related throttle/yaw processing into the subTaskRcCommand() before rc interpolation to match the ordering that existed previously.  The fixes introduced by #5905 (throttle spikes) are still retained.
---
 src/main/fc/fc_core.c | 41 +++++++++++++++++++++--------------------
 1 file changed, 21 insertions(+), 20 deletions(-)

diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 80691b06c1..564cc9b440 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -910,26 +910,6 @@ static FAST_CODE_NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
 #ifdef USE_GPS_RESCUE
     updateGPSRescueState();
 #endif
-    
-    // If we're armed, at minimum throttle, and we do arming via the
-    // sticks, do not process yaw input from the rx.  We do this so the
-    // motors do not spin up while we are trying to arm or disarm.
-    // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
-    if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
-#ifndef USE_QUAD_MIXER_ONLY
-#ifdef USE_SERVOS
-                && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
-#endif
-                && mixerConfig()->mixerMode != MIXER_AIRPLANE
-                && mixerConfig()->mixerMode != MIXER_FLYING_WING
-#endif
-    ) {
-        resetYawAxis();
-    }
-
-    if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
-        rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
-    }
 
 #ifdef USE_SDCARD
     afatfs_poll();
@@ -984,6 +964,27 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
 
 static void subTaskRcCommand(timeUs_t currentTimeUs)
 {
+
+    // If we're armed, at minimum throttle, and we do arming via the
+    // sticks, do not process yaw input from the rx.  We do this so the
+    // motors do not spin up while we are trying to arm or disarm.
+    // Allow yaw control for tricopters if the user wants the servo to move even when unarmed.
+    if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck
+#ifndef USE_QUAD_MIXER_ONLY
+#ifdef USE_SERVOS
+                && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo)
+#endif
+                && mixerConfig()->mixerMode != MIXER_AIRPLANE
+                && mixerConfig()->mixerMode != MIXER_FLYING_WING
+#endif
+    ) {
+        resetYawAxis();
+    }
+
+    if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
+        rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value);
+    }
+
     processRcCommand();
     UNUSED(currentTimeUs);
 }