diff --git a/radio/src/opentx.cpp b/radio/src/opentx.cpp index 78563178e..acccefe41 100644 --- a/radio/src/opentx.cpp +++ b/radio/src/opentx.cpp @@ -1666,28 +1666,40 @@ void opentxResume() void instantTrim() { - int16_t anas_0[MAX_INPUTS]; + int16_t anas_0[MAX_INPUTS]; evalInputs(e_perout_mode_notrainer | e_perout_mode_nosticks); memcpy(anas_0, anas, sizeof(anas_0)); evalInputs(e_perout_mode_notrainer); - for (uint8_t stick=0; sticksrcRaw-MIXSRC_Rud == stick) { - delta = anas[ed->chn] - anas_0[ed->chn]; - break; + uint8_t trimFlightMode = getTrimFlightMode(mixerCurrentFlightMode, stick); + for (uint8_t i = 0; i < MAX_EXPOS; i++) { + ExpoData * expo = expoAddress(i); + if (!EXPO_VALID(expo)) + break; // end of list + if (stick == expo->srcRaw - MIXSRC_FIRST_STICK) { + if (expo->carryTrim < 0) { + // only default trims will be taken into account + addTrim = false; + break; + } + auto newDelta = anas[expo->chn] - anas_0[expo->chn]; + if (addTrim && delta != newDelta) { + // avoid 2 different delta values + addTrim = false; + break; + } + addTrim = true; + delta = newDelta; } } - if (abs(delta) >= INSTANT_TRIM_MARGIN) { + if (addTrim && abs(delta) >= INSTANT_TRIM_MARGIN) { int16_t trim = limit(TRIM_EXTENDED_MIN, (delta + trims[stick]) / 2, TRIM_EXTENDED_MAX); - setTrimValue(trim_phase, stick, trim); + setTrimValue(trimFlightMode, stick, trim); } } }