From 057883da83c4302621a8ea721ada29edece30bc1 Mon Sep 17 00:00:00 2001 From: 3djc <5167938+3djc@users.noreply.github.com> Date: Sat, 4 Jul 2026 10:33:29 +0200 Subject: [PATCH] feat: allow IMU inversion (#7505) --- .../firmwares/edgetx/yaml_generalsettings.cpp | 5 +++ companion/src/firmwares/generalsettings.h | 2 + companion/src/generaledit/hardware.cpp | 19 ++++++++ radio/src/datastructs.h | 10 +++-- radio/src/datastructs_private.h | 1 + radio/src/gui/colorlcd/radio/hw_inputs.cpp | 23 ++++++++++ radio/src/gyro.cpp | 44 +++++++++++++++++++ radio/src/gyro.h | 9 ++++ .../src/storage/yaml/yaml_datastructs_f16.cpp | 1 + .../storage/yaml/yaml_datastructs_pa01.cpp | 1 + .../storage/yaml/yaml_datastructs_st16.cpp | 1 + .../src/storage/yaml/yaml_datastructs_t15.cpp | 1 + .../storage/yaml/yaml_datastructs_t15pro.cpp | 1 + .../src/storage/yaml/yaml_datastructs_t22.cpp | 1 + .../storage/yaml/yaml_datastructs_tx15.cpp | 1 + .../yaml/yaml_datastructs_tx16smk3.cpp | 1 + .../src/storage/yaml/yaml_datastructs_x10.cpp | 1 + .../storage/yaml/yaml_datastructs_xlites.cpp | 1 + radio/src/targets/tx15/hal.h | 2 + radio/src/targets/tx16smk3/hal.h | 2 + 20 files changed, 124 insertions(+), 3 deletions(-) diff --git a/companion/src/firmwares/edgetx/yaml_generalsettings.cpp b/companion/src/firmwares/edgetx/yaml_generalsettings.cpp index b49bb692e..551621e5d 100644 --- a/companion/src/firmwares/edgetx/yaml_generalsettings.cpp +++ b/companion/src/firmwares/edgetx/yaml_generalsettings.cpp @@ -365,6 +365,7 @@ Node convert::encode(const GeneralSettings& rhs) if (Boards::getCapability(board, Board::HasIMU)) { node["imuMax"] = rhs.imuMax; node["imuOffset"] = rhs.imuOffset; + node["imuInvert"] = (rhs.imuInvertX ? 1 : 0) | (rhs.imuInvertY ? 2 : 0); } // OneBit sampling (X9D only?) @@ -714,6 +715,10 @@ bool convert::decode(const Node& node, GeneralSettings& rhs) node["imuMax"] >> rhs.imuMax; node["imuOffset"] >> rhs.imuOffset; + int imuInvert = 0; + node["imuInvert"] >> imuInvert; + rhs.imuInvertX = imuInvert & 0x1; + rhs.imuInvertY = imuInvert & 0x2; // OneBit sampling (X9D only?) node["uartSampleMode"] >> rhs.uartSampleMode; diff --git a/companion/src/firmwares/generalsettings.h b/companion/src/firmwares/generalsettings.h index c0d553f75..05d89b8f1 100644 --- a/companion/src/firmwares/generalsettings.h +++ b/companion/src/firmwares/generalsettings.h @@ -384,6 +384,8 @@ class GeneralSettings { char registrationId[REGISTRATION_ID_LEN + 1]; int imuMax; int imuOffset; + bool imuInvertX; + bool imuInvertY; int uartSampleMode; int pwrOnSpeed; diff --git a/companion/src/generaledit/hardware.cpp b/companion/src/generaledit/hardware.cpp index 72a7b447b..58377f12c 100644 --- a/companion/src/generaledit/hardware.cpp +++ b/companion/src/generaledit/hardware.cpp @@ -107,6 +107,25 @@ HardwarePanel::HardwarePanel(QWidget * parent, GeneralSettings & generalSettings addParams(); } + if (Boards::getCapability(board, Board::HasIMU)) { + addSection(tr("IMU")); + addLabel(""); + addLabel(tr("Invert")); + addParams(); + + addLabel(tr("X")); + AutoCheckBox *imuInvertX = new AutoCheckBox(this); + imuInvertX->setField(generalSettings.imuInvertX, this); + params->append(imuInvertX); + addParams(); + + addLabel(tr("Y")); + AutoCheckBox *imuInvertY = new AutoCheckBox(this); + imuInvertY->setField(generalSettings.imuInvertY, this); + params->append(imuInvertY); + addParams(); + } + count = Boards::getCapability(board, Board::Inputs); if (count > 0) { diff --git a/radio/src/datastructs.h b/radio/src/datastructs.h index 2f89dbda1..68cf59c57 100644 --- a/radio/src/datastructs.h +++ b/radio/src/datastructs.h @@ -77,11 +77,15 @@ static inline void check_struct() #endif #if defined(PCBXLITES) - CHKSIZE(RadioData, 950); + CHKSIZE(RadioData, 951); #elif defined(RADIO_ST16) || defined(PCBPA01) || defined(RADIO_TX15) || defined(RADIO_T15PRO) || defined(RADIO_TX16SMK3) || defined(RADIO_T22) - CHKSIZE(RadioData, 1181); + CHKSIZE(RadioData, 1182); #elif defined(COLORLCD) - CHKSIZE(RadioData, 1061); + #if defined(IMU) + CHKSIZE(RadioData, 1062); + #else + CHKSIZE(RadioData, 1061); + #endif #elif defined(RADIO_GX12) CHKSIZE(RadioData, 1068); #else diff --git a/radio/src/datastructs_private.h b/radio/src/datastructs_private.h index c7367c6e4..00785562f 100644 --- a/radio/src/datastructs_private.h +++ b/radio/src/datastructs_private.h @@ -1128,6 +1128,7 @@ PACK(struct RadioData { #if defined(IMU) NOBACKUP(int8_t imuMax); NOBACKUP(int8_t imuOffset); + NOBACKUP(uint8_t imuInvert); // user inversion, default off; XORed with hal.h IMU_INVERT_X/Y at apply time (bit0=X, bit1=Y) #endif #if defined(COLORLCD) diff --git a/radio/src/gui/colorlcd/radio/hw_inputs.cpp b/radio/src/gui/colorlcd/radio/hw_inputs.cpp index 843754983..5748c1ed2 100644 --- a/radio/src/gui/colorlcd/radio/hw_inputs.cpp +++ b/radio/src/gui/colorlcd/radio/hw_inputs.cpp @@ -25,10 +25,14 @@ #include "choice.h" #include "edgetx.h" #include "getset_helpers.h" +#if defined(IMU) + #include "gyro.h" +#endif #include "hal/adc_driver.h" #include "hal/switch_driver.h" #include "layout.h" #include "static.h" +#include "strhelpers.h" #include "switches.h" #include "textedit.h" @@ -94,6 +98,25 @@ HWSticks::HWSticks(Window* parent) : Window(parent, {0, 0, LV_PCT(100), LV_SIZE_ return std::to_string(value ? 2 << (value - 1) : 0); }); #endif + +#if defined(IMU) + int imuRow = max_sticks; +#if defined(STICK_DEAD_ZONE) + imuRow += 1; +#endif + for (int a = 0; a < 2; a++) { + int row = imuRow + a; + new StaticText(this, {0, S_Y(row) + yo + PAD_MEDIUM, S_LBL_W, 0}, + getStringAtIndex(STR_IMU_VSRCRAW, a)); + new ToggleSwitch( + this, {S_INV_X, S_Y(row) + yo, S_INV_W, 0}, + [=]() -> uint8_t { return (uint8_t)getImuInversion(a); }, + [=](int8_t newValue) { + setImuInversion(a, newValue); + SET_DIRTY(); + }); + } +#endif } HWPots::HWPots(Window* parent) : diff --git a/radio/src/gyro.cpp b/radio/src/gyro.cpp index f623c221a..283e4fba9 100644 --- a/radio/src/gyro.cpp +++ b/radio/src/gyro.cpp @@ -102,6 +102,41 @@ static void gyroFilter(etx_imu_data_t* raw) } } +// Fixed hardware mounting correction from the target hal.h (X = bit 0, Y = bit 1). +// This accounts for how the chip is physically positioned and is always applied. +static uint8_t imuHwInvertMask() +{ + uint8_t mask = 0; +#if defined(IMU_INVERT_X) + mask |= (1 << IMU_AXIS_X); +#endif +#if defined(IMU_INVERT_Y) + mask |= (1 << IMU_AXIS_Y); +#endif + return mask; +} + +// User-selectable inversion, stored verbatim and defaulting to off. +bool getImuInversion(uint8_t axis) +{ + return (g_eeGeneral.imuInvert >> axis) & 1; +} + +void setImuInversion(uint8_t axis, bool value) +{ + if (value) + g_eeGeneral.imuInvert |= (1 << axis); + else + g_eeGeneral.imuInvert &= ~(1 << axis); +} + +// Effective inversion applied to sensor data: hardware correction XOR user choice. +static bool imuAxisInverted(uint8_t axis) +{ + uint8_t effective = g_eeGeneral.imuInvert ^ imuHwInvertMask(); + return (effective >> axis) & 1; +} + void gyroStart(imu_read_fn fn) { readFn = fn; @@ -126,6 +161,15 @@ void gyroWakeup() errors = 0; + if (imuAxisInverted(IMU_AXIS_X)) { + raw.accel_x = -raw.accel_x; + raw.gyro_x = -raw.gyro_x; + } + if (imuAxisInverted(IMU_AXIS_Y)) { + raw.accel_y = -raw.accel_y; + raw.gyro_y = -raw.gyro_y; + } + gyroFilter(&raw); raw_ax = (int16_t)filteredAccel[0]; diff --git a/radio/src/gyro.h b/radio/src/gyro.h index e852ed925..d6fa8ec34 100644 --- a/radio/src/gyro.h +++ b/radio/src/gyro.h @@ -29,6 +29,15 @@ #define IMU_OFFSET_MIN -30 #define IMU_OFFSET_MAX 10 +#define IMU_AXIS_X 0 +#define IMU_AXIS_Y 1 + +// User-selectable IMU axis inversion, stored verbatim and defaulting to off. +// The fixed hardware mounting correction (IMU_INVERT_X/Y in the target hal.h) is +// applied on top of this internally, so the user setting is independent of it. +bool getImuInversion(uint8_t axis); +void setImuInversion(uint8_t axis, bool value); + void gyroStart(imu_read_fn fn); void gyroWakeup(); void gyroSetIMU_X(int16_t offset, int16_t range); diff --git a/radio/src/storage/yaml/yaml_datastructs_f16.cpp b/radio/src/storage/yaml/yaml_datastructs_f16.cpp index 1bc49eb77..ba914c983 100644 --- a/radio/src/storage/yaml/yaml_datastructs_f16.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_f16.cpp @@ -435,6 +435,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_pa01.cpp b/radio/src/storage/yaml/yaml_datastructs_pa01.cpp index 1f31ce303..2e7bb9c16 100644 --- a/radio/src/storage/yaml/yaml_datastructs_pa01.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_pa01.cpp @@ -458,6 +458,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_UNSIGNED( "stickDeadZone", 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_st16.cpp b/radio/src/storage/yaml/yaml_datastructs_st16.cpp index 18ffed16d..9039a1cd3 100644 --- a/radio/src/storage/yaml/yaml_datastructs_st16.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_st16.cpp @@ -458,6 +458,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_UNSIGNED( "stickDeadZone", 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_t15.cpp b/radio/src/storage/yaml/yaml_datastructs_t15.cpp index c826d265e..92eca63de 100644 --- a/radio/src/storage/yaml/yaml_datastructs_t15.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_t15.cpp @@ -443,6 +443,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_t15pro.cpp b/radio/src/storage/yaml/yaml_datastructs_t15pro.cpp index 18ffed16d..9039a1cd3 100644 --- a/radio/src/storage/yaml/yaml_datastructs_t15pro.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_t15pro.cpp @@ -458,6 +458,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_UNSIGNED( "stickDeadZone", 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_t22.cpp b/radio/src/storage/yaml/yaml_datastructs_t22.cpp index 0c26f31a5..1e86c2f41 100644 --- a/radio/src/storage/yaml/yaml_datastructs_t22.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_t22.cpp @@ -458,6 +458,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_tx15.cpp b/radio/src/storage/yaml/yaml_datastructs_tx15.cpp index 18ffed16d..9039a1cd3 100644 --- a/radio/src/storage/yaml/yaml_datastructs_tx15.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_tx15.cpp @@ -458,6 +458,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_UNSIGNED( "stickDeadZone", 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_tx16smk3.cpp b/radio/src/storage/yaml/yaml_datastructs_tx16smk3.cpp index 60eeb8c37..5992e5ec4 100644 --- a/radio/src/storage/yaml/yaml_datastructs_tx16smk3.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_tx16smk3.cpp @@ -459,6 +459,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_x10.cpp b/radio/src/storage/yaml/yaml_datastructs_x10.cpp index ad2938cec..e15e3a323 100644 --- a/radio/src/storage/yaml/yaml_datastructs_x10.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_x10.cpp @@ -434,6 +434,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_STRING("selectedTheme", 26), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), diff --git a/radio/src/storage/yaml/yaml_datastructs_xlites.cpp b/radio/src/storage/yaml/yaml_datastructs_xlites.cpp index 4365672c6..a5a3b3b4a 100644 --- a/radio/src/storage/yaml/yaml_datastructs_xlites.cpp +++ b/radio/src/storage/yaml/yaml_datastructs_xlites.cpp @@ -372,6 +372,7 @@ static const struct YamlNode struct_RadioData[] = { YAML_PADDING( 3 ), YAML_SIGNED( "imuMax", 8 ), YAML_SIGNED( "imuOffset", 8 ), + YAML_UNSIGNED( "imuInvert", 8 ), YAML_SIGNED_CUST( "backlightSrc", 10, r_mixSrcRawEx, w_mixSrcRawEx ), YAML_SIGNED( "radioGFDisabled", 1 ), YAML_SIGNED( "radioTrainerDisabled", 1 ), diff --git a/radio/src/targets/tx15/hal.h b/radio/src/targets/tx15/hal.h index 105ef5b38..1637c6e1b 100644 --- a/radio/src/targets/tx15/hal.h +++ b/radio/src/targets/tx15/hal.h @@ -402,6 +402,8 @@ SAI1_Block_A: PDM_CLOCK (1 MHz bit-clock output on SAI1_CK1 / PE5, !FLYSKY_GIMBA // IMU #define IMU_I2C_BUS I2C_Bus_1 #define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13 +#define IMU_INVERT_X // X axis mounted reversed (default, user-overridable) +#define IMU_INVERT_Y // Y axis mounted reversed (default, user-overridable) // IMU_INT_EXTI IRQ #if !defined(USE_EXTI15_10_IRQ) diff --git a/radio/src/targets/tx16smk3/hal.h b/radio/src/targets/tx16smk3/hal.h index ec540649d..5c0d3e3d7 100644 --- a/radio/src/targets/tx16smk3/hal.h +++ b/radio/src/targets/tx16smk3/hal.h @@ -601,6 +601,8 @@ USART6: INTMODULE_USART #define IMU_I2C_BUS I2C_Bus_2 #define IMU_INT_GPIO GPIO_PIN(GPIOG, 13) // PG.13 +#define IMU_INVERT_X // X axis mounted reversed (default, user-overridable) +#define IMU_INVERT_Y // Y axis mounted reversed (default, user-overridable) #if !defined(USE_EXTI15_10_IRQ) #define USE_EXTI15_10_IRQ #define EXTI15_10_IRQ_Priority 6