diff --git a/radio/src/mixer.cpp b/radio/src/mixer.cpp index 56abde109..4992e608f 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,16 @@ 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); + 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); diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index f6b60cb7e..e127cf2e0 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1119,7 +1119,7 @@ void checkTrims() else { phase = getTrimFlightMode(mixerCurrentFlightMode, idx); before = getTrimValue(phase, idx); - thro = (idx==THR_STICK && g_model.thrTrim); + thro = (idx == (g_model.getThrottleStickTrimSource() - MIXSRC_FIRST_TRIM) && g_model.thrTrim); } #else phase = getTrimFlightMode(mixerCurrentFlightMode, idx); @@ -1824,24 +1824,27 @@ void moveTrimsToOffsets() // copy state of 3 primary to subtrim pauseMixerCalculations(); evalFlightModeMixes(e_perout_mode_noinput, 0); // do output loop - zero input sticks and trims - for (uint8_t i=0; icarryTrim = TRIM_ELE; + expo = expoAddress(ELE_STICK); + expo->carryTrim = TRIM_THR; + + anaInValues[THR_STICK] = 0; + setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, 100); + setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, -100); + evalMixes(1); + EXPECT_EQ(channelOutputs[2], 200); // THR output value is reflecting 100 Ele trim + moveTrimsToOffsets(); + evalMixes(1); + EXPECT_EQ(channelOutputs[2], 200); // THR output value remains unchanged + EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 0); // back to neutral + EXPECT_EQ(g_model.limitData[2].offset, 195); // value transferred + EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 0); // back to neutral + EXPECT_EQ(g_model.limitData[1].offset, -195); // value transferred +} + +TEST_F(TrimsTest, MoveTrimsToOffsetsWithCrosstrimsAndTrimIdle) +{ + // Trim idle only + // Cross trims + g_model.limitData[2].offset = 0; + g_model.limitData[1].offset = 0; + g_model.thrTrim = 1; + g_model.thrTrimSw = MIXSRC_TrimEle - MIXSRC_FIRST_TRIM; + ExpoData *expo = expoAddress(THR_STICK); + expo->carryTrim = TRIM_ELE; + expo = expoAddress(ELE_STICK); + expo->carryTrim = TRIM_THR; + + anaInValues[THR_STICK] = -1024; // Min stick + setTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM, 100); + setTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM, -100); + evalMixes(1); + EXPECT_EQ(channelOutputs[2], -574); // THR output value is reflecting 100 ele trim idle + moveTrimsToOffsets(); + + // Trim affecting Throttle (now Ele because of crosstrims) should not be affected + EXPECT_EQ(getTrimValue(0, MIXSRC_TrimEle - MIXSRC_FIRST_TRIM), 100); // unchanged + EXPECT_EQ(g_model.limitData[2].offset, 0); // THR chan offset unchanged + + // Other trims should + EXPECT_EQ(getTrimValue(0, MIXSRC_TrimThr - MIXSRC_FIRST_TRIM), 0); // back to neutral + EXPECT_EQ(g_model.limitData[1].offset, -195); // Ele chan offset transfered + evalMixes(1); + EXPECT_EQ(channelOutputs[2], -574); // THR output value is still reflecting 100 trim idle +} + TEST_F(TrimsTest, InstantTrim) { anaInValues[AIL_STICK] = 50;