Add anti gravity as feature and mode

[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 <letter>.
+  '_<_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.
+
+

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)

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);

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;

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) {

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)

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);

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
+    // Calculate and 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_ROLL] =
-    scaledAxisPIDf[FD_PITCH] = constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] * throttle + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
+        constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING,
-    scaledAxisPIDf[FD_YAW] = constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW] * throttle) / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
+        -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;

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;