From 2d9306652d38e2d0c5ca23c5e79d2f6bc2d5a874 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Fri, 31 Mar 2017 15:55:31 +0200 Subject: [PATCH] Add anti gravity as feature and mode --- [activeBoxIdCount++] = BOXAIRMODE | 233 ++++++++++++++++++ src/main/fc/config.c | 2 +- src/main/fc/config.h | 1 + src/main/fc/fc_msp.c | 7 +- src/main/fc/fc_rc.c | 3 +- src/main/fc/rc_controls.c | 4 + src/main/fc/rc_controls.h | 3 +- src/main/flight/mixer.c | 14 +- ...xes[CHECKBOX_ITEM_COUNT + 1] = BOXHORIZON, | 106 ++++++++ 9 files changed, 365 insertions(+), 8 deletions(-) create mode 100644 [activeBoxIdCount++] = BOXAIRMODE create mode 100644 tatic const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {{123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = BOXHORIZON, diff --git a/[activeBoxIdCount++] = BOXAIRMODE b/[activeBoxIdCount++] = BOXAIRMODE new file mode 100644 index 0000000000..2a8e3e7233 --- /dev/null +++ b/[activeBoxIdCount++] = BOXAIRMODE @@ -0,0 +1,233 @@ + + SSUUMMMMAARRYY OOFF LLEESSSS CCOOMMMMAANNDDSS + + Commands marked with * may be preceded by a number, _N. + Notes in parentheses indicate the behavior if _N is given. + A key preceded by a caret indicates the Ctrl key; thus ^K is ctrl-K. + + h H Display this help. + q :q Q :Q ZZ Exit. + --------------------------------------------------------------------------- + + MMOOVVIINNGG + + e ^E j ^N CR * Forward one line (or _N lines). + y ^Y k ^K ^P * Backward one line (or _N lines). + f ^F ^V SPACE * Forward one window (or _N lines). + b ^B ESC-v * Backward one window (or _N lines). + z * Forward one window (and set window to _N). + w * Backward one window (and set window to _N). + ESC-SPACE * Forward one window, but don't stop at end-of-file. + d ^D * Forward one half-window (and set half-window to _N). + u ^U * Backward one half-window (and set half-window to _N). + ESC-) RightArrow * Left one half screen width (or _N positions). + ESC-( LeftArrow * Right one half screen width (or _N positions). + F Forward forever; like "tail -f". + ESC-F Like F but stop when search pattern is found. + r ^R ^L Repaint screen. + R Repaint screen, discarding buffered input. + --------------------------------------------------- + Default "window" is the screen height. + Default "half-window" is half of the screen height. + --------------------------------------------------------------------------- + + SSEEAARRCCHHIINNGG + + /_p_a_t_t_e_r_n * Search forward for (_N-th) matching line. + ?_p_a_t_t_e_r_n * Search backward for (_N-th) matching line. + n * Repeat previous search (for _N-th occurrence). + N * Repeat previous search in reverse direction. + ESC-n * Repeat previous search, spanning files. + ESC-N * Repeat previous search, reverse dir. & spanning files. + ESC-u Undo (toggle) search highlighting. + &_p_a_t_t_e_r_n * Display only matching lines + --------------------------------------------------- + A search pattern may be preceded by one or more of: + ^N or ! Search for NON-matching lines. + ^E or * Search multiple files (pass thru END OF FILE). + ^F or @ Start search at FIRST file (for /) or last file (for ?). + ^K Highlight matches, but don't move (KEEP position). + ^R Don't use REGULAR EXPRESSIONS. + --------------------------------------------------------------------------- + + JJUUMMPPIINNGG + + g < ESC-< * Go to first line in file (or line _N). + G > ESC-> * Go to last line in file (or line _N). + p % * Go to beginning of file (or _N percent into file). + t * Go to the (_N-th) next tag. + T * Go to the (_N-th) previous tag. + { ( [ * Find close bracket } ) ]. + } ) ] * Find open bracket { ( [. + ESC-^F _<_c_1_> _<_c_2_> * Find close bracket _<_c_2_>. + ESC-^B _<_c_1_> _<_c_2_> * Find open bracket _<_c_1_> + --------------------------------------------------- + Each "find close bracket" command goes forward to the close bracket + matching the (_N-th) open bracket in the top line. + Each "find open bracket" command goes backward to the open bracket + matching the (_N-th) close bracket in the bottom line. + + m_<_l_e_t_t_e_r_> Mark the current position with . + '_<_l_e_t_t_e_r_> Go to a previously marked position. + '' Go to the previous position. + ^X^X Same as '. + --------------------------------------------------- + A mark is any upper-case or lower-case letter. + Certain marks are predefined: + ^ means beginning of the file + $ means end of the file + --------------------------------------------------------------------------- + + CCHHAANNGGIINNGG FFIILLEESS + + :e [_f_i_l_e] Examine a new file. + ^X^V Same as :e. + :n * Examine the (_N-th) next file from the command line. + :p * Examine the (_N-th) previous file from the command line. + :x * Examine the first (or _N-th) file from the command line. + :d Delete the current file from the command line list. + = ^G :f Print current file name. + --------------------------------------------------------------------------- + + MMIISSCCEELLLLAANNEEOOUUSS CCOOMMMMAANNDDSS + + -_<_f_l_a_g_> Toggle a command line option [see OPTIONS below]. + --_<_n_a_m_e_> Toggle a command line option, by name. + __<_f_l_a_g_> Display the setting of a command line option. + ___<_n_a_m_e_> Display the setting of an option, by name. + +_c_m_d Execute the less cmd each time a new file is examined. + + !_c_o_m_m_a_n_d Execute the shell command with $SHELL. + |XX_c_o_m_m_a_n_d Pipe file between current pos & mark XX to shell command. + v Edit the current file with $VISUAL or $EDITOR. + V Print version number of "less". + --------------------------------------------------------------------------- + + OOPPTTIIOONNSS + + Most options may be changed either on the command line, + or from within less by using the - or -- command. + Options may be given in one of two forms: either a single + character preceded by a -, or a name preceded by --. + + -? ........ --help + Display help (from command line). + -a ........ --search-skip-screen + Search skips current screen. + -A ........ --SEARCH-SKIP-SCREEN + Search starts just after target line. + -b [_N] .... --buffers=[_N] + Number of buffers. + -B ........ --auto-buffers + Don't automatically allocate buffers for pipes. + -c ........ --clear-screen + Repaint by clearing rather than scrolling. + -d ........ --dumb + Dumb terminal. + -D [_x_n_._n] . --color=_x_n_._n + Set screen colors. (MS-DOS only) + -e -E .... --quit-at-eof --QUIT-AT-EOF + Quit at end of file. + -f ........ --force + Force open non-regular files. + -F ........ --quit-if-one-screen + Quit if entire file fits on first screen. + -g ........ --hilite-search + Highlight only last match for searches. + -G ........ --HILITE-SEARCH + Don't highlight any matches for searches. + -h [_N] .... --max-back-scroll=[_N] + Backward scroll limit. + -i ........ --ignore-case + Ignore case in searches that do not contain uppercase. + -I ........ --IGNORE-CASE + Ignore case in all searches. + -j [_N] .... --jump-target=[_N] + Screen position of target lines. + -J ........ --status-column + Display a status column at left edge of screen. + -k [_f_i_l_e] . --lesskey-file=[_f_i_l_e] + Use a lesskey file. + -K --quit-on-intr + Exit less in response to ctrl-C. + -L ........ --no-lessopen + Ignore the LESSOPEN environment variable. + -m -M .... --long-prompt --LONG-PROMPT + Set prompt style. + -n -N .... --line-numbers --LINE-NUMBERS + Don't use line numbers. + -o [_f_i_l_e] . --log-file=[_f_i_l_e] + Copy to log file (standard input only). + -O [_f_i_l_e] . --LOG-FILE=[_f_i_l_e] + Copy to log file (unconditionally overwrite). + -p [_p_a_t_t_e_r_n] --pattern=[_p_a_t_t_e_r_n] + Start at pattern (from command line). + -P [_p_r_o_m_p_t] --prompt=[_p_r_o_m_p_t] + Define new prompt. + -q -Q .... --quiet --QUIET --silent --SILENT + Quiet the terminal bell. + -r -R .... --raw-control-chars --RAW-CONTROL-CHARS + Output "raw" control characters. + -s ........ --squeeze-blank-lines + Squeeze multiple blank lines. + -S ........ --chop-long-lines + Chop (truncate) long lines rather than wrapping. + -t [_t_a_g] .. --tag=[_t_a_g] + Find a tag. + -T [_t_a_g_s_f_i_l_e] --tag-file=[_t_a_g_s_f_i_l_e] + Use an alternate tags file. + -u -U .... --underline-special --UNDERLINE-SPECIAL + Change handling of backspaces. + -V ........ --version + Display the version number of "less". + -w ........ --hilite-unread + Highlight first new line after forward-screen. + -W ........ --HILITE-UNREAD + Highlight first new line after any forward movement. + -x [_N[,...]] --tabs=[_N[,...]] + Set tab stops. + -X ........ --no-init + Don't use termcap init/deinit strings. + -y [_N] .... --max-forw-scroll=[_N] + Forward scroll limit. + -z [_N] .... --window=[_N] + Set size of window. + -" [_c[_c]] . --quotes=[_c[_c]] + Set shell quote characters. + -~ ........ --tilde + Don't display tildes after end of file. + -# [_N] .... --shift=[_N] + Horizontal scroll amount (0 = one half screen width) + ........ --no-keypad + Don't send termcap keypad init/deinit strings. + ........ --follow-name + The F command changes files if the input file is renamed. + ........ --use-backslash + Subsequent options use backslash as escape char. + + + --------------------------------------------------------------------------- + + LLIINNEE EEDDIITTIINNGG + + These keys can be used to edit text being entered + on the "command line" at the bottom of the screen. + + RightArrow ..................... ESC-l ... Move cursor right one character. + LeftArrow ...................... ESC-h ... Move cursor left one character. + ctrl-RightArrow ESC-RightArrow ESC-w ... Move cursor right one word. + ctrl-LeftArrow ESC-LeftArrow ESC-b ... Move cursor left one word. + HOME ........................... ESC-0 ... Move cursor to start of line. + END ............................ ESC-$ ... Move cursor to end of line. + BACKSPACE ................................ Delete char to left of cursor. + DELETE ......................... ESC-x ... Delete char under cursor. + ctrl-BACKSPACE ESC-BACKSPACE ........... Delete word to left of cursor. + ctrl-DELETE .... ESC-DELETE .... ESC-X ... Delete word under cursor. + ctrl-U ......... ESC (MS-DOS only) ....... Delete entire line. + UpArrow ........................ ESC-k ... Retrieve previous command line. + DownArrow ...................... ESC-j ... Retrieve next command line. + TAB ...................................... Complete filename & cycle. + SHIFT-TAB ...................... ESC-TAB Complete filename & reverse cycle. + ctrl-L ................................... Complete filename, list all. + + diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 176e4978c8..8015846a32 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -179,7 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yawRateAccelLimit = 10.0f; pidProfile->rateAccelLimit = 0.0f; pidProfile->itermThrottleThreshold = 350; - pidProfile->itermAcceleratorGain = 1.0f; + pidProfile->itermAcceleratorGain = 3.0f; } void resetProfile(profile_t *profile) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 0fe4f9e616..82b6835a39 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -56,6 +56,7 @@ typedef enum { FEATURE_RX_SPI = 1 << 25, FEATURE_SOFTSPI = 1 << 26, FEATURE_ESC_SENSOR = 1 << 27, + FEATURE_ANTI_GRAVITY = 1 << 28, } features_e; void beeperOffSet(uint32_t mask); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 80da41769f..d31e888324 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -123,7 +123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXANGLE, "ANGLE;", 1 }, { BOXHORIZON, "HORIZON;", 2 }, { BOXBARO, "BARO;", 3 }, - //{ BOXVARIO, "VARIO;", 4 }, + { BOXANTIGRAVITY, "ANTI GRAVITY;", 4 }, { BOXMAG, "MAG;", 5 }, { BOXHEADFREE, "HEADFREE;", 6 }, { BOXHEADADJ, "HEADADJ;", 7 }, @@ -327,6 +327,10 @@ void initActiveBoxIds(void) activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; } + if (!feature(FEATURE_ANTI_GRAVITY)) { + activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY; + } + if (sensors(SENSOR_ACC)) { activeBoxIds[activeBoxIdCount++] = BOXANGLE; activeBoxIds[activeBoxIdCount++] = BOXHORIZON; @@ -444,6 +448,7 @@ static uint32_t packFlightModeFlags(void) IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY)) << BOXANTIGRAVITY | IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; uint32_t ret = 0; diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index d7e50f6276..3f0c055c8f 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -181,8 +181,9 @@ void processRcCommand(void) if (isRXDataNew) { currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000); - if (currentProfile->pidProfile.itermAcceleratorGain > 1.0f) + if (isAntiGravityModeActive()) { checkForThrottleErrorResetState(currentRxRefreshRate); + } } if (rxConfig()->rcInterpolation || flightModeFlags) { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 0f8a79b01e..5966f16d48 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -73,6 +73,10 @@ bool isAirmodeActive(void) { return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); } +bool isAntiGravityModeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY)); +} + void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { #ifndef BLACKBOX #define UNUSED(x) (void)(x) diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index e9c88f874b..ef10d37577 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -24,7 +24,7 @@ typedef enum { BOXANGLE, BOXHORIZON, BOXBARO, - // BOXVARIO, + BOXANTIGRAVITY, BOXMAG, BOXHEADFREE, BOXHEADADJ, @@ -271,6 +271,7 @@ typedef struct adjustmentProfile_s { } adjustmentProfile_t; bool isAirmodeActive(void); +bool isAntiGravityModeActive(void); void resetAdjustmentStates(void); void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 85df4adbd1..6f3847c027 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -479,10 +479,16 @@ void mixTable(pidProfile_t *pidProfile) const float motorOutputRange = motorOutputMax - motorOutputMin; float scaledAxisPIDf[3]; - // Limit the PIDsum - scaledAxisPIDf[FD_ROLL] = constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] * throttle + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); - scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] * throttle + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); - scaledAxisPIDf[FD_YAW] = constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] * throttle) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); + // Calculate and Limit the PIDsum + scaledAxisPIDf[FD_ROLL] = + constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, + -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[FD_PITCH] = + constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, + -pidProfile->pidSumLimit, pidProfile->pidSumLimit); + scaledAxisPIDf[FD_YAW] = + constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING, + -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); // Calculate voltage compensation const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f; diff --git a/tatic const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {{123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = BOXHORIZON, b/tatic const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {{123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = BOXHORIZON, new file mode 100644 index 0000000000..6c01dedd68 --- /dev/null +++ b/tatic const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {{123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { 123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = BOXHORIZON, @@ -0,0 +1,106 @@ +diff --git a/src/main/fc/config.h b/src/main/fc/config.h +index 0fe4f9e..9850d2b 100644 +--- a/src/main/fc/config.h ++++ b/src/main/fc/config.h +@@ -56,6 +56,7 @@ typedef enum { + FEATURE_RX_SPI = 1 << 25, + FEATURE_SOFTSPI = 1 << 26, + FEATURE_ESC_SENSOR = 1 << 27, ++ FEATURE_TEST = 1 << 28, + } features_e; +  + void beeperOffSet(uint32_t mask); +diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c +index 80da417..4f3dfe7 100755 +--- a/src/main/fc/fc_msp.c ++++ b/src/main/fc/fc_msp.c +@@ -123,7 +123,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { + { BOXANGLE, "ANGLE;", 1 }, + { BOXHORIZON, "HORIZON;", 2 }, + { BOXBARO, "BARO;", 3 }, +- //{ BOXVARIO, "VARIO;", 4 }, ++ { BOXTEST, "TEST;", 4 }, + { BOXMAG, "MAG;", 5 }, + { BOXHEADFREE, "HEADFREE;", 6 }, + { BOXHEADADJ, "HEADADJ;", 7 }, +@@ -327,6 +327,10 @@ void initActiveBoxIds(void) + activeBoxIds[activeBoxIdCount++] = BOXAIRMODE; + } +  ++ if (!feature(FEATURE_TEST)) { ++ activeBoxIds[activeBoxIdCount++] = BOXTEST; ++ } ++ + if (sensors(SENSOR_ACC)) { + activeBoxIds[activeBoxIdCount++] = BOXANGLE; + activeBoxIds[activeBoxIdCount++] = BOXHORIZON; +@@ -444,6 +448,7 @@ static uint32_t packFlightModeFlags(void) + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOX)) << BOXBLACKBOX | + IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE | ++ IS_ENABLED(IS_RC_MODE_ACTIVE(BOXTEST)) << BOXTEST | + IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX; +  + uint32_t ret = 0; +diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c +index 0f8a79b..4c9baba 100644 +--- a/src/main/fc/rc_controls.c ++++ b/src/main/fc/rc_controls.c +@@ -73,6 +73,10 @@ bool isAirmodeActive(void) { + return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE)); + } +  ++bool isTestmodeActive(void) { ++ return (IS_RC_MODE_ACTIVE(BOXTEST) || feature(FEATURE_TEST)); ++} ++ + void blackboxLogInflightAdjustmentEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue) { + #ifndef BLACKBOX + #define UNUSED(x) (void)(x) +diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h +index e9c88f8..dd76247 100644 +--- a/src/main/fc/rc_controls.h ++++ b/src/main/fc/rc_controls.h +@@ -24,7 +24,7 @@ typedef enum { + BOXANGLE, + BOXHORIZON, + BOXBARO, +- // BOXVARIO, ++ BOXTEST, + BOXMAG, + BOXHEADFREE, + BOXHEADADJ, +@@ -271,6 +271,7 @@ typedef struct adjustmentProfile_s { + } adjustmentProfile_t; +  + bool isAirmodeActive(void); ++bool isTestmodeActive(void); + void resetAdjustmentStates(void); + void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges); + void processRcAdjustments(controlRateConfig_t *controlRateConfig, struct rxConfig_s *rxConfig); +diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c +index 85df4ad..b282ccf 100755 +--- a/src/main/flight/mixer.c ++++ b/src/main/flight/mixer.c +@@ -479,10 +479,17 @@ void mixTable(pidProfile_t *pidProfile) + const float motorOutputRange = motorOutputMax - motorOutputMin; +  + float scaledAxisPIDf[3]; +- // Limit the PIDsum +- scaledAxisPIDf[FD_ROLL] = constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] * throttle + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); +- scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] * throttle + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit); +- scaledAxisPIDf[FD_YAW] = constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] * throttle) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); ++ float itermDampener = (isTestmodeActive()) ? constrainf(throttle * 1.8f, 0.1f, throttle) : 1.0f; ++ // Calculate and Limit the PIDsum ++ scaledAxisPIDf[FD_ROLL] = ++ constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] * itermDampener + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING, ++ -pidProfile->pidSumLimit, pidProfile->pidSumLimit); ++ scaledAxisPIDf[FD_PITCH] = ++ constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] * itermDampener + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, ++ -pidProfile->pidSumLimit, pidProfile->pidSumLimit); ++ scaledAxisPIDf[FD_YAW] = ++ constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] * itermDampener) / PID_MIXER_SCALING, ++ -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw); +  + // Calculate voltage compensation + const float vbatCompensationFactor = (batteryConfig && pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;