From d91b43b1a9da33c1698c8223c4b54efe4fa980d8 Mon Sep 17 00:00:00 2001
From: 3djc <lesitewebdejc@hotmail.com>
Date: Thu, 26 Nov 2020 18:10:21 +0100
Subject: [PATCH] patch throttle trim

---
 radio/src/mixer.cpp | 31 +++++++++++++++++++++++--------
 1 file changed, 23 insertions(+), 8 deletions(-)

diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp
index 56abde109..4f15ff68f 100644
--- a/radio/src/mixer.cpp
+++ b/radio/src/mixer.cpp
@@ -539,14 +539,25 @@ int getStickTrimValue(int stick, int stickValue)
   return trim;
 }
 
-int getSourceTrimValue(int source, int stickValue=0)
+int getSourceTrimOrigin(int source)
 {
   if (source >= MIXSRC_Rud && source <= MIXSRC_Ail)
-    return getStickTrimValue(source - MIXSRC_Rud, stickValue);
+    return source - MIXSRC_Rud;
   else if (source >= MIXSRC_FIRST_INPUT && source <= MIXSRC_LAST_INPUT)
-    return getStickTrimValue(virtualInputsTrims[source - MIXSRC_FIRST_INPUT], stickValue);
+    return virtualInputsTrims[source - MIXSRC_FIRST_INPUT];
   else
+    return -1;
+}
+
+int getSourceTrimValue(int source, int stickValue=0)
+{
+  auto origin = getSourceTrimOrigin(source);
+  if (origin >= 0) {
+    return getStickTrimValue(origin, stickValue);
+  }
+  else {
     return 0;
+  }
 }
 
 uint8_t mixerCurrentFlightMode;
@@ -741,13 +752,17 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
       }
 
       if (applyOffsetAndCurve) {
-
-        //========== TRIMS ================
-        if (!(mode & e_perout_mode_notrims)) {
-          if (md->carryTrim == 0) {
-            v += getSourceTrimValue(md->srcRaw, v);
+        bool applyTrims = !(mode & e_perout_mode_notrims);
+        if (!applyTrims && g_model.thrTrim) {
+          auto origin = getSourceTrimOrigin(md->srcRaw);
+          TRACE("thrTrim=%d srcRaw=%d origin=%d stick=%d", g_model.thrTrim, md->srcRaw, origin, g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM);
+          if (origin == g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM) {
+            applyTrims = true;
           }
         }
+        if (applyTrims && md->carryTrim == 0) {
+          v += getSourceTrimValue(md->srcRaw, v);
+        }
       }
 
       int32_t weight = GET_GVAR_PREC1(MD_WEIGHT(md), GV_RANGELARGE_NEG, GV_RANGELARGE, mixerCurrentFlightMode);