From 189becfbb27044571aa18b09fda6c49eafc262f6 Mon Sep 17 00:00:00 2001
From: bsongis <bsongis@gmail.com>
Date: Thu, 31 Jul 2014 11:22:18 +0200
Subject: [PATCH] Fixes #1569

---
 radio/src/mixer.cpp  | 26 +++++++++++++++++---------
 radio/src/opentx.cpp | 10 ++++++++--
 2 files changed, 25 insertions(+), 11 deletions(-)

diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp
index f9bf966d7..1d06d2d25 100755
--- a/radio/src/mixer.cpp
+++ b/radio/src/mixer.cpp
@@ -509,17 +509,17 @@ void evalInputs(uint8_t mode)
 
 #if !defined(PCBTARANIS)
       rawAnas[ch] = v;
-      anas[ch] = v; //set values for mixer
+      anas[ch] = v; // set values for mixer
 #endif
     }
   }
 
-  /* TRIMs */
-  evalTrims();
-
   /* EXPOs */
   applyExpos(anas, mode);
 
+  /* TRIMs */
+  evalTrims(); // when no virtual inputs, the trims need the anas array calculated above (when throttle trim enabled)
+
   if (mode == e_perout_mode_normal) {
 #if !defined(CPUARM)
     anaCenter &= g_model.beepANACenter;
@@ -749,11 +749,19 @@ void evalFlightModeMixes(uint8_t mode, uint8_t tick10ms)
             else
               mix_trim = -1;
             if (mix_trim >= 0) {
-              int16_t trim = trims[mix_trim];
-              if (mix_trim == THR_STICK && g_model.throttleReversed)
-                v -= trim;
-              else
-                v += trim;
+              int32_t trim = trims[mix_trim];
+              if (mix_trim == THR_STICK) {
+                if (g_model.thrTrim) {
+                  if (g_model.throttleReversed) {
+                    trim = -trim;
+                  }
+                  trim = ((trim-2*TRIM_MIN) * (RESX-v)) >> (RESX_SHIFT+1);
+                }
+                if (g_model.throttleReversed) {
+                  trim = -trim;
+                }
+              }
+              v += trim;
             }
           }
 #else
diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp
index d6157ed3a..c603d3396 100644
--- a/radio/src/opentx.cpp
+++ b/radio/src/opentx.cpp
@@ -1659,10 +1659,16 @@ FORCEINLINE void evalTrims()
   for (uint8_t i=0; i<NUM_STICKS; i++) {
     // do trim -> throttle trim if applicable
     int16_t trim = getTrimValue(phase, i);
+#if !defined(PCBTARANIS)
     if (i==THR_STICK && g_model.thrTrim) {
-      trim = (((g_model.throttleReversed)?(int32_t)(trim+TRIM_MIN):(int32_t)(trim-TRIM_MIN)) * (RESX-getValue(MIXSRC_Thr))) >> (RESX_SHIFT+1);
+      if (g_model.throttleReversed)
+        trim = -trim;
+      int16_t v = anas[i];
+      int32_t vv = ((int32_t)trim-TRIM_MIN)*(RESX-v)>>(RESX_SHIFT+1);
+      trim = vv;
     }
-    else if (trimsCheckTimer > 0) {
+#endif
+    if (trimsCheckTimer > 0) {
       trim = 0;
     }