From 6007cfd3c7a4e5a4150768d1d2183b64201488aa Mon Sep 17 00:00:00 2001
From: philmoz <philmoz@users.noreply.github.com>
Date: Tue, 8 Oct 2024 11:43:14 +1100
Subject: [PATCH] fix(bw): unshifted trims on XLite radios should always adjust
 elevator and aileron (#5580)

---
 radio/src/boards/generic_stm32/inputs.cpp |  9 +------
 radio/src/keys.cpp                        | 30 ++++++++++++++++++++---
 radio/src/targets/simu/simpgmspace.cpp    | 11 +++------
 3 files changed, 31 insertions(+), 19 deletions(-)

diff --git a/radio/src/boards/generic_stm32/inputs.cpp b/radio/src/boards/generic_stm32/inputs.cpp
index c1c3341c049..80856a0b0a3 100644
--- a/radio/src/boards/generic_stm32/inputs.cpp
+++ b/radio/src/boards/generic_stm32/inputs.cpp
@@ -41,12 +41,5 @@ __weak uint32_t readKeys()
 
 __weak uint32_t readTrims()
 {
-  uint32_t trims = _read_trims();
-
-#if defined(PCBXLITE)
-  if (_read_keys() & (1 << KEY_SHIFT))
-    return ((trims & 0x03) << 6) | ((trims & 0x0c) << 2);
-#endif
-
-  return trims;
+  return _read_trims();
 }
diff --git a/radio/src/keys.cpp b/radio/src/keys.cpp
index 77cd9a13a4a..f2298fa1b67 100644
--- a/radio/src/keys.cpp
+++ b/radio/src/keys.cpp
@@ -28,7 +28,7 @@
 #include "hal/rotary_encoder.h"
 #include "dataconstants.h"
 
-#if !defined(BOOT) && defined(USE_HATS_AS_KEYS)
+#if !defined(BOOT) && (defined(USE_HATS_AS_KEYS) || defined(PCBXLITE))
 #include "opentx.h"
 #endif
 
@@ -267,6 +267,30 @@ bool waitKeysReleased()
   return true;
 }
 
+#if defined(PCBXLITE) && !defined(BOOT)
+uint32_t _readTrims()
+{
+  uint32_t trims = readTrims();
+
+  uint8_t lr = trims & 0x3;
+  uint8_t ud = trims & 0xc;
+  bool shift = readKeys() & (1 << KEY_SHIFT);
+  // Mode 1 or 2 - AIL on right stick
+  bool ailRight = g_eeGeneral.stickMode < 2;
+  // Mode 2 or 4 - ELE on right stick
+  bool eleRight = (g_eeGeneral.stickMode & 1) == 1;
+  // Ensure non-shifted trims are AIL and ELE
+  if (ailRight == !shift) lr <<= 6;
+  if (eleRight == !shift) ud <<= 2;
+
+  return lr | ud;
+}
+
+#define READ_TRIMS() _readTrims()
+#else
+#define READ_TRIMS() readTrims()
+#endif
+
 bool keyDown()
 {
   return readKeys() || readTrims();
@@ -274,7 +298,7 @@ bool keyDown()
 
 bool trimDown(uint8_t idx)
 {
-  return readTrims() & (1 << idx);
+  return READ_TRIMS() & (1 << idx);
 }
 
 uint8_t keysGetState(uint8_t key)
@@ -421,7 +445,7 @@ bool keysPollingCycle()
     trims_input = readTrims();
   }
 #else
-  trims_input = readTrims();
+  trims_input = READ_TRIMS();
 #endif
 
   for (int i = 0; i < MAX_KEYS; i++) {
diff --git a/radio/src/targets/simu/simpgmspace.cpp b/radio/src/targets/simu/simpgmspace.cpp
index b3264c1137b..f75f7f6899e 100644
--- a/radio/src/targets/simu/simpgmspace.cpp
+++ b/radio/src/targets/simu/simpgmspace.cpp
@@ -476,21 +476,16 @@ uint32_t readKeys()
 
 uint32_t readTrims()
 {
-  uint32_t result = 0;
+  uint32_t trims = 0;
 
   for (int i = 0; i < keysGetMaxTrims() * 2; i++) {
     if (trimsStates[i]) {
       // TRACE("trim pressed %d", i);
-      result |= 1 << i;
+      trims |= 1 << i;
     }
   }
 
-#if defined(PCBXLITE)
-  if (keysStates[KEY_SHIFT])
-    result = ((result & 0x03) << 6) | ((result & 0x0c) << 2);
-#endif
-
-  return result;
+  return trims;
 }
 
 int usbPlugged() { return false; }