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; }