Filter instant trim only default trims (#7683)

Filter instant trim only default trims
This commit is contained in:
DanielGeA
2020-10-03 11:59:20 +02:00
committed by GitHub
parent 41bb035ce5
commit ca7d038aba
+22 -10
View File
@@ -1673,21 +1673,33 @@ void instantTrim()
evalInputs(e_perout_mode_notrainer); evalInputs(e_perout_mode_notrainer);
for (uint8_t stick = 0; stick < NUM_STICKS; stick++) { for (uint8_t stick = 0; stick < NUM_STICKS; stick++) {
if (stick!=THR_STICK) { if (stick != THR_STICK) { // don't instant trim the throttle stick
// don't instant trim the throttle stick bool addTrim = false;
uint8_t trim_phase = getTrimFlightMode(mixerCurrentFlightMode, stick);
int16_t delta = 0; int16_t delta = 0;
for (int e=0; e<MAX_EXPOS; e++) { uint8_t trimFlightMode = getTrimFlightMode(mixerCurrentFlightMode, stick);
ExpoData * ed = expoAddress(e); for (uint8_t i = 0; i < MAX_EXPOS; i++) {
if (!EXPO_VALID(ed)) break; // end of list ExpoData * expo = expoAddress(i);
if (ed->srcRaw-MIXSRC_Rud == stick) { if (!EXPO_VALID(expo))
delta = anas[ed->chn] - anas_0[ed->chn]; 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; break;
} }
auto newDelta = anas[expo->chn] - anas_0[expo->chn];
if (addTrim && delta != newDelta) {
// avoid 2 different delta values
addTrim = false;
break;
} }
if (abs(delta) >= INSTANT_TRIM_MARGIN) { addTrim = true;
delta = newDelta;
}
}
if (addTrim && abs(delta) >= INSTANT_TRIM_MARGIN) {
int16_t trim = limit<int16_t>(TRIM_EXTENDED_MIN, (delta + trims[stick]) / 2, TRIM_EXTENDED_MAX); int16_t trim = limit<int16_t>(TRIM_EXTENDED_MIN, (delta + trims[stick]) / 2, TRIM_EXTENDED_MAX);
setTrimValue(trim_phase, stick, trim); setTrimValue(trimFlightMode, stick, trim);
} }
} }
} }