src/main/flight/pid.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include <platform.h>
  23
  24 #include "build/build_config.h"
  25 #include "build/debug.h"
  26
  27 #include "common/axis.h"
  28 #include "common/filter.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31 #include "common/fp_pid.h"
  32
  33 #include "config/parameter_group.h"
  34 #include "config/parameter_group_ids.h"
  35
  36 #include "fc/config.h"
  37 #include "fc/controlrate_profile.h"
  38 #include "fc/rc_controls.h"
  39 #include "fc/rc_modes.h"
  40 #include "fc/runtime_config.h"
  41 #include "fc/settings.h"
  42
  43 #include "flight/pid.h"
  44 #include "flight/imu.h"
  45 #include "flight/mixer.h"
  46 #include "flight/mixer_profile.h"
  47 #include "flight/rpm_filter.h"
  48 #include "flight/kalman.h"
  49 #include "flight/smith_predictor.h"
  50
  51 #include "io/gps.h"
  52
  53 #include "navigation/navigation.h"
  54
  55 #include "rx/rx.h"
  56
  57 #include "sensors/battery.h"
  58 #include "sensors/sensors.h"
  59 #include "sensors/gyro.h"
  60 #include "sensors/acceleration.h"
  61 #include "sensors/compass.h"
  62 #include "sensors/pitotmeter.h"
  63
  64 #include "scheduler/scheduler.h"
  65
  66 #include "programming/logic_condition.h"
  67
  68 typedef struct {
  69     float aP;
  70     float aI;
  71     float aD;
  72     float aFF;
  73     timeMs_t targetOverThresholdTimeMs;
  74 } fwPidAttenuation_t;
  75
  76 typedef struct {
  77     uint8_t axis;
  78     float kP;   // Proportional gain
  79     float kI;   // Integral gain
  80     float kD;   // Derivative gain
  81     float kFF;  // Feed-forward gain
  82     float kCD;  // Control Derivative
  83     float kT;   // Back-calculation tracking gain
  84
  85     float gyroRate;
  86     float rateTarget;
  87
  88     // Rate integrator
  89     float errorGyroIf;
  90     float errorGyroIfLimit;
  91
  92     // Used for ANGLE filtering (PT1, we don't need super-sharpness here)
  93     pt1Filter_t angleFilterState;
  94
  95     // Rate filtering
  96     rateLimitFilter_t axisAccelFilter;
  97     pt1Filter_t ptermLpfState;
  98     filter_t dtermLpfState;
  99
 100     float stickPosition;
 101
 102     float previousRateTarget;
 103     float previousRateGyro;
 104
 105 #ifdef USE_D_BOOST
 106     pt1Filter_t dBoostLpf;
 107     biquadFilter_t dBoostGyroLpf;
 108     float dBoostTargetAcceleration;
 109 #endif
 110     filterApply4FnPtr ptermFilterApplyFn;
 111     bool itermLimitActive;
 112     bool itermFreezeActive;
 113
 114     pt3Filter_t rateTargetFilter;
 115
 116     smithPredictor_t smithPredictor;
 117
 118     fwPidAttenuation_t attenuation;
 119 } pidState_t;
 120
 121 STATIC_FASTRAM bool pidFiltersConfigured = false;
 122 static EXTENDED_FASTRAM float headingHoldCosZLimit;
 123 static EXTENDED_FASTRAM int16_t headingHoldTarget;
 124 static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
 125 static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
 126
 127 // Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
 128 STATIC_FASTRAM bool pidGainsUpdateRequired;
 129 FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
 130
 131 #ifdef USE_BLACKBOX
 132 int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT];
 133 int32_t axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT];
 134 int32_t axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT];
 135 int32_t axisPID_F[FLIGHT_DYNAMICS_INDEX_COUNT];
 136 int32_t axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT];
 137 #endif
 138
 139 static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
 140 static EXTENDED_FASTRAM pt1Filter_t windupLpf[XYZ_AXIS_COUNT];
 141 static EXTENDED_FASTRAM uint8_t itermRelax;
 142
 143 #ifdef USE_ANTIGRAVITY
 144 static EXTENDED_FASTRAM pt1Filter_t antigravityThrottleLpf;
 145 static EXTENDED_FASTRAM float antigravityThrottleHpf;
 146 static EXTENDED_FASTRAM float antigravityGain;
 147 static EXTENDED_FASTRAM float antigravityAccelerator;
 148 #endif
 149
 150 #define D_BOOST_GYRO_LPF_HZ 80    // Biquad lowpass input cutoff to peak D around propwash frequencies
 151 #define D_BOOST_LPF_HZ 7          // PT1 lowpass cutoff to smooth the boost effect
 152
 153 #ifdef USE_D_BOOST
 154 static EXTENDED_FASTRAM float dBoostMin;
 155 static EXTENDED_FASTRAM float dBoostMax;
 156 static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
 157 #endif
 158
 159 static EXTENDED_FASTRAM uint8_t yawLpfHz;
 160 static EXTENDED_FASTRAM float motorItermWindupPoint;
 161 static EXTENDED_FASTRAM float antiWindupScaler;
 162 #ifdef USE_ANTIGRAVITY
 163 static EXTENDED_FASTRAM float iTermAntigravityGain;
 164 #endif
 165 static EXTENDED_FASTRAM uint8_t usedPidControllerType;
 166
 167 typedef void (*pidControllerFnPtr)(pidState_t *pidState, float dT, float dT_inv);
 168 static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
 169 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 170 static EXTENDED_FASTRAM bool restartAngleHoldMode = true;
 171 static EXTENDED_FASTRAM bool angleHoldIsLevel = false;
 172
 173 #define FIXED_WING_LEVEL_TRIM_MAX_ANGLE 10.0f // Max angle auto trimming can demand
 174 #define FIXED_WING_LEVEL_TRIM_DIVIDER 50.0f
 175 #define FIXED_WING_LEVEL_TRIM_MULTIPLIER 1.0f / FIXED_WING_LEVEL_TRIM_DIVIDER
 176 #define FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT FIXED_WING_LEVEL_TRIM_DIVIDER * FIXED_WING_LEVEL_TRIM_MAX_ANGLE
 177
 178 static EXTENDED_FASTRAM float fixedWingLevelTrim;
 179 static EXTENDED_FASTRAM pidController_t fixedWingLevelTrimController;
 180
 181 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 9);
 182
 183 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
 184         .bank_mc = {
 185             .pid = {
 186                 [PID_ROLL] =  { SETTING_MC_P_ROLL_DEFAULT, SETTING_MC_I_ROLL_DEFAULT, SETTING_MC_D_ROLL_DEFAULT, SETTING_MC_CD_ROLL_DEFAULT },
 187                 [PID_PITCH] = { SETTING_MC_P_PITCH_DEFAULT, SETTING_MC_I_PITCH_DEFAULT, SETTING_MC_D_PITCH_DEFAULT, SETTING_MC_CD_PITCH_DEFAULT },
 188                 [PID_YAW] =   { SETTING_MC_P_YAW_DEFAULT, SETTING_MC_I_YAW_DEFAULT, SETTING_MC_D_YAW_DEFAULT, SETTING_MC_CD_YAW_DEFAULT },
 189                 [PID_LEVEL] = {
 190                     .P = SETTING_MC_P_LEVEL_DEFAULT,          // Self-level strength
 191                     .I = SETTING_MC_I_LEVEL_DEFAULT,          // Self-leveing low-pass frequency (0 - disabled)
 192                     .D = SETTING_MC_D_LEVEL_DEFAULT,          // 75% horizon strength
 193                     .FF = 0,
 194                 },
 195                 [PID_HEADING] = { SETTING_NAV_MC_HEADING_P_DEFAULT, 0, 0, 0 },
 196                 [PID_POS_XY] = {
 197                     .P = SETTING_NAV_MC_POS_XY_P_DEFAULT,     // NAV_POS_XY_P * 100
 198                     .I = 0,
 199                     .D = 0,
 200                     .FF = 0,
 201                 },
 202                 [PID_VEL_XY] = {
 203                     .P = SETTING_NAV_MC_VEL_XY_P_DEFAULT,     // NAV_VEL_XY_P * 20
 204                     .I = SETTING_NAV_MC_VEL_XY_I_DEFAULT,     // NAV_VEL_XY_I * 100
 205                     .D = SETTING_NAV_MC_VEL_XY_D_DEFAULT,     // NAV_VEL_XY_D * 100
 206                     .FF = SETTING_NAV_MC_VEL_XY_FF_DEFAULT,   // NAV_VEL_XY_D * 100
 207                 },
 208                 [PID_POS_Z] = {
 209                     .P = SETTING_NAV_MC_POS_Z_P_DEFAULT,      // NAV_POS_Z_P * 100
 210                     .I = 0,                                   // not used
 211                     .D = 0,                                   // not used
 212                     .FF = 0,
 213                 },
 214                 [PID_VEL_Z] = {
 215                     .P = SETTING_NAV_MC_VEL_Z_P_DEFAULT,      // NAV_VEL_Z_P * 66.7
 216                     .I = SETTING_NAV_MC_VEL_Z_I_DEFAULT,      // NAV_VEL_Z_I * 20
 217                     .D = SETTING_NAV_MC_VEL_Z_D_DEFAULT,      // NAV_VEL_Z_D * 100
 218                     .FF = 0,
 219                 },
 220                 [PID_POS_HEADING] = {
 221                     .P = 0,
 222                     .I = 0,
 223                     .D = 0,
 224                     .FF = 0
 225                 }
 226             }
 227         },
 228
 229         .bank_fw = {
 230             .pid = {
 231                 [PID_ROLL] =  { SETTING_FW_P_ROLL_DEFAULT, SETTING_FW_I_ROLL_DEFAULT, 0, SETTING_FW_FF_ROLL_DEFAULT },
 232                 [PID_PITCH] =  { SETTING_FW_P_PITCH_DEFAULT, SETTING_FW_I_PITCH_DEFAULT, 0, SETTING_FW_FF_PITCH_DEFAULT },
 233                 [PID_YAW] =  { SETTING_FW_P_YAW_DEFAULT, SETTING_FW_I_YAW_DEFAULT, 0, SETTING_FW_FF_YAW_DEFAULT },
 234                 [PID_LEVEL] = {
 235                     .P = SETTING_FW_P_LEVEL_DEFAULT,          // Self-level strength
 236                     .I = SETTING_FW_I_LEVEL_DEFAULT,          // Self-leveing low-pass frequency (0 - disabled)
 237                     .D = SETTING_FW_D_LEVEL_DEFAULT,          // 75% horizon strength
 238                     .FF = 0,
 239                 },
 240                 [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 },
 241                 [PID_POS_Z] = {
 242                     .P = SETTING_NAV_FW_POS_Z_P_DEFAULT,      // FW_POS_Z_P * 100
 243                     .I = SETTING_NAV_FW_POS_Z_I_DEFAULT,      // FW_POS_Z_I * 100
 244                     .D = SETTING_NAV_FW_POS_Z_D_DEFAULT,      // FW_POS_Z_D * 100
 245                     .FF = 0,
 246                 },
 247                 [PID_POS_XY] = {
 248                     .P = SETTING_NAV_FW_POS_XY_P_DEFAULT,     // FW_POS_XY_P * 100
 249                     .I = SETTING_NAV_FW_POS_XY_I_DEFAULT,     // FW_POS_XY_I * 100
 250                     .D = SETTING_NAV_FW_POS_XY_D_DEFAULT,     // FW_POS_XY_D * 100
 251                     .FF = 0,
 252                 },
 253                 [PID_POS_HEADING] = {
 254                     .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT,
 255                     .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT,
 256                     .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT,
 257                     .FF = 0
 258                 }
 259             }
 260         },
 261
 262         .dterm_lpf_type = SETTING_DTERM_LPF_TYPE_DEFAULT,
 263         .dterm_lpf_hz = SETTING_DTERM_LPF_HZ_DEFAULT,
 264         .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT,
 265
 266         .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT,
 267
 268         .axisAccelerationLimitYaw = SETTING_RATE_ACCEL_LIMIT_YAW_DEFAULT,
 269         .axisAccelerationLimitRollPitch = SETTING_RATE_ACCEL_LIMIT_ROLL_PITCH_DEFAULT,
 270
 271         .heading_hold_rate_limit = SETTING_HEADING_HOLD_RATE_LIMIT_DEFAULT,
 272
 273         .max_angle_inclination[FD_ROLL] = SETTING_MAX_ANGLE_INCLINATION_RLL_DEFAULT,
 274         .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT,
 275         .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT,
 276
 277         .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT,
 278         .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT,
 279         .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT,
 280         .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT,
 281
 282         .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT,
 283         .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT,
 284         .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT,
 285         .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT,
 286         .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT,
 287         .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT,
 288
 289 #ifdef USE_D_BOOST
 290         .dBoostMin = SETTING_D_BOOST_MIN_DEFAULT,
 291         .dBoostMax = SETTING_D_BOOST_MAX_DEFAULT,
 292         .dBoostMaxAtAlleceleration = SETTING_D_BOOST_MAX_AT_ACCELERATION_DEFAULT,
 293         .dBoostGyroDeltaLpfHz = SETTING_D_BOOST_GYRO_DELTA_LPF_HZ_DEFAULT,
 294 #endif
 295
 296 #ifdef USE_ANTIGRAVITY
 297         .antigravityGain = SETTING_ANTIGRAVITY_GAIN_DEFAULT,
 298         .antigravityAccelerator = SETTING_ANTIGRAVITY_ACCELERATOR_DEFAULT,
 299         .antigravityCutoff = SETTING_ANTIGRAVITY_CUTOFF_LPF_HZ_DEFAULT,
 300 #endif
 301
 302         .pidControllerType = SETTING_PID_TYPE_DEFAULT,
 303         .navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT,
 304         .controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT,
 305
 306         .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT,
 307         .fixedWingLevelTrimGain = SETTING_FW_LEVEL_PITCH_GAIN_DEFAULT,
 308
 309         .fwAltControlResponseFactor = SETTING_NAV_FW_ALT_CONTROL_RESPONSE_DEFAULT,
 310 #ifdef USE_SMITH_PREDICTOR
 311         .smithPredictorStrength = SETTING_SMITH_PREDICTOR_STRENGTH_DEFAULT,
 312         .smithPredictorDelay = SETTING_SMITH_PREDICTOR_DELAY_DEFAULT,
 313         .smithPredictorFilterHz = SETTING_SMITH_PREDICTOR_LPF_HZ_DEFAULT,
 314 #endif
 315         .fwItermLockTimeMaxMs = SETTING_FW_ITERM_LOCK_TIME_MAX_MS_DEFAULT,
 316         .fwItermLockRateLimit = SETTING_FW_ITERM_LOCK_RATE_THRESHOLD_DEFAULT,
 317 );
 318
 319 bool pidInitFilters(void)
 320 {
 321     const uint32_t refreshRate = getLooptime();
 322
 323     if (refreshRate == 0) {
 324         return false;
 325     }
 326
 327     for (int axis = 0; axis < 3; ++ axis) {
 328         initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
 329     }
 330
 331     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 332         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
 333     }
 334
 335 #ifdef USE_ANTIGRAVITY
 336     pt1FilterInit(&antigravityThrottleLpf, pidProfile()->antigravityCutoff, US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
 337 #endif
 338
 339 #ifdef USE_D_BOOST
 340     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 341         biquadFilterInitLPF(&pidState[axis].dBoostGyroLpf, pidProfile()->dBoostGyroDeltaLpfHz, getLooptime());
 342     }
 343 #endif
 344
 345     if (pidProfile()->controlDerivativeLpfHz) {
 346         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 347             pt3FilterInit(&pidState[axis].rateTargetFilter, pt3FilterGain(pidProfile()->controlDerivativeLpfHz, US2S(refreshRate)));
 348         }
 349     }
 350
 351 #ifdef USE_SMITH_PREDICTOR
 352     smithPredictorInit(
 353         &pidState[FD_ROLL].smithPredictor,
 354         pidProfile()->smithPredictorDelay,
 355         pidProfile()->smithPredictorStrength,
 356         pidProfile()->smithPredictorFilterHz,
 357         getLooptime()
 358     );
 359     smithPredictorInit(
 360         &pidState[FD_PITCH].smithPredictor,
 361         pidProfile()->smithPredictorDelay,
 362         pidProfile()->smithPredictorStrength,
 363         pidProfile()->smithPredictorFilterHz,
 364         getLooptime()
 365     );
 366     smithPredictorInit(
 367         &pidState[FD_YAW].smithPredictor,
 368         pidProfile()->smithPredictorDelay,
 369         pidProfile()->smithPredictorStrength,
 370         pidProfile()->smithPredictorFilterHz,
 371         getLooptime()
 372     );
 373 #endif
 374
 375     pidFiltersConfigured = true;
 376
 377     return true;
 378 }
 379
 380 void pidResetTPAFilter(void)
 381 {
 382     if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
 383         pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlRateProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
 384         pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
 385     }
 386 }
 387
 388 void pidResetErrorAccumulators(void)
 389 {
 390     // Reset R/P/Y integrator
 391     for (int axis = 0; axis < 3; axis++) {
 392         pidState[axis].errorGyroIf = 0.0f;
 393         pidState[axis].errorGyroIfLimit = 0.0f;
 394     }
 395 }
 396
 397 void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
 398 {
 399     pidState[axis].errorGyroIf -= delta;
 400     pidState[axis].errorGyroIfLimit -= delta;
 401 }
 402
 403 float getTotalRateTarget(void)
 404 {
 405     return calc_length_pythagorean_3D(pidState[FD_ROLL].rateTarget, pidState[FD_PITCH].rateTarget, pidState[FD_YAW].rateTarget);
 406 }
 407
 408 float getAxisIterm(uint8_t axis)
 409 {
 410     return pidState[axis].errorGyroIf;
 411 }
 412
 413 static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
 414 {
 415     stick = constrain(stick, -500, 500);
 416     return scaleRangef((float) stick, -500.0f, 500.0f, (float) -maxInclination, (float) maxInclination);
 417 }
 418
 419 int16_t pidAngleToRcCommand(float angleDeciDegrees, int16_t maxInclination)
 420 {
 421     angleDeciDegrees = constrainf(angleDeciDegrees, (float) -maxInclination, (float) maxInclination);
 422     return scaleRangef((float) angleDeciDegrees, (float) -maxInclination, (float) maxInclination, -500.0f, 500.0f);
 423 }
 424
 425 /*
 426 Map stick positions to desired rotatrion rate in given axis.
 427 Rotation rate in dps at full stick deflection is defined by axis rate measured in dps/10
 428 Rate 20 means 200dps at full stick deflection
 429 */
 430 float pidRateToRcCommand(float rateDPS, uint8_t rate)
 431 {
 432     const float maxRateDPS = rate * 10.0f;
 433     return scaleRangef(rateDPS, -maxRateDPS, maxRateDPS, -500.0f, 500.0f);
 434 }
 435
 436 float pidRcCommandToRate(int16_t stick, uint8_t rate)
 437 {
 438     const float maxRateDPS = rate * 10.0f;
 439     return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS);
 440 }
 441
 442 static float calculateFixedWingTPAFactor(uint16_t throttle)
 443 {
 444     float tpaFactor;
 445
 446     // tpa_rate is amount of curve TPA applied to PIDs
 447     // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
 448     if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) {
 449         if (throttle > getThrottleIdleValue()) {
 450             // Calculate TPA according to throttle
 451             tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
 452
 453             // Limit to [0.5; 2] range
 454             tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
 455         }
 456         else {
 457             tpaFactor = 2.0f;
 458         }
 459
 460         // Attenuate TPA curve according to configured amount
 461         tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->throttle.dynPID / 100.0f);
 462     }
 463     else {
 464         tpaFactor = 1.0f;
 465     }
 466
 467     return tpaFactor;
 468 }
 469
 470 static float calculateMultirotorTPAFactor(void)
 471 {
 472     float tpaFactor;
 473
 474     // TPA should be updated only when TPA is actually set
 475     if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) {
 476         tpaFactor = 1.0f;
 477     } else if (rcCommand[THROTTLE] < getMaxThrottle()) {
 478         tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
 479     } else {
 480         tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
 481     }
 482
 483     return tpaFactor;
 484 }
 485
 486 void schedulePidGainsUpdate(void)
 487 {
 488     pidGainsUpdateRequired = true;
 489 }
 490
 491 void updatePIDCoefficients(void)
 492 {
 493     STATIC_FASTRAM uint16_t prevThrottle = 0;
 494
 495     // Check if throttle changed. Different logic for fixed wing vs multirotor
 496     if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
 497         uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
 498         if (filteredThrottle != prevThrottle) {
 499             prevThrottle = filteredThrottle;
 500             pidGainsUpdateRequired = true;
 501         }
 502     }
 503     else {
 504         if (rcCommand[THROTTLE] != prevThrottle) {
 505             prevThrottle = rcCommand[THROTTLE];
 506             pidGainsUpdateRequired = true;
 507         }
 508     }
 509
 510 #ifdef USE_ANTIGRAVITY
 511     if (usedPidControllerType == PID_TYPE_PID) {
 512         antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
 513         iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
 514     }
 515 #endif
 516
 517     /*
 518      * Compute stick position in range of [-1.0f : 1.0f] without deadband and expo
 519      */
 520     for (int axis = 0; axis < 3; axis++) {
 521         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
 522     }
 523
 524     // If nothing changed - don't waste time recalculating coefficients
 525     if (!pidGainsUpdateRequired) {
 526         return;
 527     }
 528
 529     const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
 530
 531     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
 532     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
 533     for (int axis = 0; axis < 3; axis++) {
 534         if (usedPidControllerType == PID_TYPE_PIFF) {
 535             // Airplanes - scale all PIDs according to TPA
 536             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER  * tpaFactor;
 537             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
 538             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
 539             pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
 540             pidState[axis].kCD = 0.0f;
 541             pidState[axis].kT  = 0.0f;
 542         }
 543         else {
 544             const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor;
 545             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
 546             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
 547             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
 548             pidState[axis].kCD = (pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA) / (getLooptime() * 0.000001f);
 549             pidState[axis].kFF = 0.0f;
 550
 551             // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
 552             if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
 553                 pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
 554             } else {
 555                 pidState[axis].kT = 0;
 556             }
 557         }
 558     }
 559
 560     pidGainsUpdateRequired = false;
 561 }
 562
 563 static float calcHorizonRateMagnitude(void)
 564 {
 565     // Figure out the raw stick positions
 566     const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL));
 567     const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH));
 568     const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f;
 569     const float modeTransitionStickPos = constrain(pidBank()->pid[PID_LEVEL].D, 0, 100) / 100.0f;
 570
 571     float horizonRateMagnitude;
 572
 573     // Calculate transition point according to stick deflection
 574     if (mostDeflectedStickPos <= modeTransitionStickPos) {
 575         horizonRateMagnitude = mostDeflectedStickPos / modeTransitionStickPos;
 576     }
 577     else {
 578         horizonRateMagnitude = 1.0f;
 579     }
 580
 581     return horizonRateMagnitude;
 582 }
 583
 584 /* ANGLE freefloat deadband (degs).Angle error only starts to increase if atttiude outside deadband. */
 585 int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
 586 {
 587     int16_t levelDatum = axis == FD_PITCH ? attitude.raw[axis] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : attitude.raw[axis];
 588     if (ABS(levelDatum) > deadband) {
 589         return levelDatum > 0 ? deadband - levelDatum : -(levelDatum + deadband);
 590     } else {
 591         return 0;
 592     }
 593 }
 594
 595 static float computePidLevelTarget(flight_dynamics_index_t axis) {
 596     // This is ROLL/PITCH, run ANGLE/HORIZON controllers
 597 #ifdef USE_PROGRAMMING_FRAMEWORK
 598     float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]);
 599 #else
 600     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
 601 #endif
 602
 603     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
 604 #ifdef USE_FW_AUTOLAND
 605     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 606 #else
 607     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
 608 #endif
 609         angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
 610     }
 611
 612     //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
 613     if (axis == FD_PITCH && STATE(AIRPLANE)) {
 614         DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
 615         DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
 616         DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
 617
 618         /*
 619          * fixedWingLevelTrim has opposite sign to rcCommand.
 620          * Positive rcCommand means nose should point downwards
 621          * Negative rcCommand mean nose should point upwards
 622          * This is counter intuitive and a natural way suggests that + should mean UP
 623          * This is why fixedWingLevelTrim has opposite sign to rcCommand
 624          * Positive fixedWingLevelTrim means nose should point upwards
 625          * Negative fixedWingLevelTrim means nose should point downwards
 626          */
 627         angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
 628         DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
 629     }
 630
 631     return angleTarget;
 632 }
 633
 634 static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
 635 {
 636     float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
 637
 638     // Soaring mode deadband inactive if pitch/roll stick not centered to allow RC stick adjustment
 639     if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
 640         angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH));
 641         if (!angleErrorDeg) {
 642             pidState->errorGyroIf = 0.0f;
 643             pidState->errorGyroIfLimit = 0.0f;
 644         }
 645     }
 646
 647     float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f);
 648
 649     // Apply simple LPF to angleRateTarget to make response less jerky
 650     // Ideas behind this:
 651     //  1) Attitude is updated at gyro rate, rateTarget for ANGLE mode is calculated from attitude
 652     //  2) If this rateTarget is passed directly into gyro-base PID controller this effectively doubles the rateError.
 653     //     D-term that is calculated from error tend to amplify this even more. Moreover, this tend to respond to every
 654     //     slightest change in attitude making self-leveling jittery
 655     //  3) Lowering LEVEL P can make the effects of (2) less visible, but this also slows down self-leveling.
 656     //  4) Human pilot response to attitude change in RATE mode is fairly slow and smooth, human pilot doesn't
 657     //     compensate for each slightest change
 658     //  5) (2) and (4) lead to a simple idea of adding a low-pass filter on rateTarget for ANGLE mode damping
 659     //     response to rapid attitude changes and smoothing out self-leveling reaction
 660     if (pidBank()->pid[PID_LEVEL].I) {
 661         // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
 662         angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidBank()->pid[PID_LEVEL].I, dT);
 663     }
 664
 665     // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
 666     if (FLIGHT_MODE(HORIZON_MODE)) {
 667         pidState->rateTarget = (1.0f - horizonRateMagnitude) * angleRateTarget + horizonRateMagnitude * pidState->rateTarget;
 668     } else {
 669         pidState->rateTarget = angleRateTarget;
 670     }
 671 }
 672
 673 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
 674 static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 675 {
 676     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 677
 678     if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) {
 679         pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT);
 680     }
 681 }
 682
 683 static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
 684     float newPTerm = rateError * pidState->kP;
 685
 686     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
 687 }
 688
 689 #ifdef USE_D_BOOST
 690 static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
 691
 692     float dBoost = 1.0f;
 693
 694     const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
 695     const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
 696     const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);
 697
 698     if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
 699         //Gyro is accelerating faster than setpoint, we want to smooth out
 700         dBoost = scaleRangef(dBoostGyroAcceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostMax);
 701     } else {
 702         //Setpoint is accelerating, we want to boost response
 703         dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, dBoostMin);
 704     }
 705
 706     dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
 707     dBoost = constrainf(dBoost, dBoostMin, dBoostMax);
 708
 709     return dBoost;
 710 }
 711 #else
 712 static float applyDBoost(pidState_t *pidState, float dT) {
 713     UNUSED(pidState);
 714     UNUSED(dT);
 715     return 1.0f;
 716 }
 717 #endif
 718
 719 static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
 720     // Calculate new D-term
 721     float newDTerm = 0;
 722     if (pidState->kD == 0) {
 723         // optimisation for when D is zero, often used by YAW axis
 724         newDTerm = 0;
 725     } else {
 726         float delta = pidState->previousRateGyro - pidState->gyroRate;
 727
 728         delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
 729
 730         // Calculate derivative
 731         newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
 732     }
 733     return(newDTerm);
 734 }
 735
 736 static void applyItermLimiting(pidState_t *pidState) {
 737     if (pidState->itermLimitActive) {
 738         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
 739     } else
 740     {
 741         pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
 742     }
 743 }
 744
 745 static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
 746     UNUSED(pidState);
 747     UNUSED(dT);
 748     UNUSED(dT_inv);
 749 }
 750
 751 static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
 752     const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
 753
 754     const float dampingFactor = attenuation(rateTarget, maxRate * pidProfile()->fwItermLockRateLimit / 100.0f);
 755
 756     /*
 757      * Iterm damping is applied (down to 0) when:
 758      * abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
 759
 760      * itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
 761      */
 762
 763     //If error is greater than 10% or max rate
 764     const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
 765
 766     //If stick (setpoint) was moved above threshold in the last 500ms
 767     if (fabsf(rateTarget) > maxRate * 0.2f) {
 768         pidState->attenuation.targetOverThresholdTimeMs = millis();
 769     }
 770
 771     //If error is below threshold, we no longer track time for lock mechanism
 772     if (!errorThresholdReached) {
 773         pidState->attenuation.targetOverThresholdTimeMs = 0;
 774     }
 775
 776     pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < pidProfile()->fwItermLockTimeMaxMs) ? 0.0f : 1.0f);
 777
 778     //P & D damping factors are always the same and based on current damping factor
 779     pidState->attenuation.aP = dampingFactor;
 780     pidState->attenuation.aD = dampingFactor;
 781
 782     if (pidState->axis == FD_ROLL) {
 783         DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
 784         DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
 785         DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
 786     }
 787 }
 788
 789 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)
 790 {
 791     const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 792
 793     const float rateError = rateTarget - pidState->gyroRate;
 794
 795     fwRateAttenuation(pidState, rateTarget, rateError);
 796
 797     const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
 798     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
 799     const float newFFTerm = rateTarget * pidState->kFF;
 800
 801     /*
 802      * Integral should be updated only if axis Iterm is not frozen
 803      */
 804     if (!pidState->itermFreezeActive) {
 805         pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
 806     }
 807
 808     applyItermLimiting(pidState);
 809
 810     const uint16_t limit = getPidSumLimit(pidState->axis);
 811
 812     if (pidProfile()->pidItermLimitPercent != 0){
 813         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
 814         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
 815     }
 816
 817     axisPID[pidState->axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
 818
 819     if (FLIGHT_MODE(SOARING_MODE) && pidState->axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
 820         if (!angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)) {
 821             axisPID[FD_PITCH] = 0;  // center pitch servo if pitch attitude within soaring mode deadband
 822         }
 823     }
 824
 825 #ifdef USE_AUTOTUNE_FIXED_WING
 826     if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
 827         autotuneFixedWingUpdate(pidState->axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
 828     }
 829 #endif
 830
 831 #ifdef USE_BLACKBOX
 832     axisPID_P[pidState->axis] = newPTerm;
 833     axisPID_I[pidState->axis] = pidState->errorGyroIf;
 834     axisPID_D[pidState->axis] = newDTerm;
 835     axisPID_F[pidState->axis] = newFFTerm;
 836     axisPID_Setpoint[pidState->axis] = rateTarget;
 837 #endif
 838
 839     pidState->previousRateGyro = pidState->gyroRate;
 840
 841 }
 842
 843 static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
 844 {
 845     if (itermRelax) {
 846         if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
 847
 848             const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
 849             const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
 850
 851             const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
 852             return itermErrorRate * itermRelaxFactor;
 853         }
 854     }
 855
 856     return itermErrorRate;
 857 }
 858
 859 static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, float dT, float dT_inv)
 860 {
 861
 862     const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 863
 864     const float rateError = rateTarget - pidState->gyroRate;
 865     const float newPTerm = pTermProcess(pidState, rateError, dT);
 866     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
 867
 868     const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
 869     const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
 870
 871     /*
 872      * Compute Control Derivative
 873      */
 874     const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD;
 875
 876     const uint16_t limit = getPidSumLimit(pidState->axis);
 877
 878     // TODO: Get feedback from mixer on available correction range for each axis
 879     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
 880     const float newOutputLimited = constrainf(newOutput, -limit, +limit);
 881
 882     float itermErrorRate = applyItermRelax(pidState->axis, rateTarget, rateError);
 883
 884 #ifdef USE_ANTIGRAVITY
 885     itermErrorRate *= iTermAntigravityGain;
 886 #endif
 887
 888     pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
 889                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
 890
 891     if (pidProfile()->pidItermLimitPercent != 0){
 892         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
 893         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
 894     }
 895
 896
 897     // Don't grow I-term if motors are at their limit
 898     applyItermLimiting(pidState);
 899
 900     axisPID[pidState->axis] = newOutputLimited;
 901
 902 #ifdef USE_BLACKBOX
 903     axisPID_P[pidState->axis] = newPTerm;
 904     axisPID_I[pidState->axis] = pidState->errorGyroIf;
 905     axisPID_D[pidState->axis] = newDTerm;
 906     axisPID_F[pidState->axis] = newCDTerm;
 907     axisPID_Setpoint[pidState->axis] = rateTarget;
 908 #endif
 909
 910     pidState->previousRateTarget = rateTarget;
 911     pidState->previousRateGyro = pidState->gyroRate;
 912 }
 913
 914 void updateHeadingHoldTarget(int16_t heading)
 915 {
 916     headingHoldTarget = heading;
 917 }
 918
 919 void resetHeadingHoldTarget(int16_t heading)
 920 {
 921     updateHeadingHoldTarget(heading);
 922     pt1FilterReset(&headingHoldRateFilter, 0.0f);
 923 }
 924
 925 int16_t getHeadingHoldTarget(void) {
 926     return headingHoldTarget;
 927 }
 928
 929 static uint8_t getHeadingHoldState(void)
 930 {
 931     // Don't apply heading hold if overall tilt is greater than maximum angle inclination
 932     if (calculateCosTiltAngle() < headingHoldCosZLimit) {
 933         return HEADING_HOLD_DISABLED;
 934     }
 935
 936     int navHeadingState = navigationGetHeadingControlState();
 937     // NAV will prevent MAG_MODE from activating, but require heading control
 938     if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
 939         // Apply maghold only if heading control is in auto mode
 940         if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
 941             return HEADING_HOLD_ENABLED;
 942         }
 943     }
 944     else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
 945         return HEADING_HOLD_ENABLED;
 946     }
 947
 948     return HEADING_HOLD_UPDATE_HEADING;
 949 }
 950
 951 /*
 952  * HEADING_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
 953  */
 954 float pidHeadingHold(float dT)
 955 {
 956     float headingHoldRate;
 957
 958     /* Convert absolute error into relative to current heading */
 959     int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
 960
 961     /* Convert absolute error into relative to current heading */
 962     if (error > 180) {
 963         error -= 360;
 964     } else if (error < -180) {
 965         error += 360;
 966     }
 967
 968     /*
 969         New MAG_HOLD controller work slightly different that previous one.
 970         Old one mapped error to rotation speed in following way:
 971             - on rate 0 it gave about 0.5dps for each degree of error
 972             - error 0 = rotation speed of 0dps
 973             - error 180 = rotation speed of 96 degrees per second
 974             - output
 975             - that gives about 2 seconds to correct any error, no matter how big. Of course, usually more because of inertia.
 976         That was making him quite "soft" for small changes and rapid for big ones that started to appear
 977         when INAV introduced real RTH and WAYPOINT that might require rapid heading changes.
 978
 979         New approach uses modified principle:
 980             - manual yaw rate is not used. MAG_HOLD is decoupled from manual input settings
 981             - instead, mag_hold_rate_limit is used. It defines max rotation speed in dps that MAG_HOLD controller can require from RateController
 982             - computed rotation speed is capped at -mag_hold_rate_limit and mag_hold_rate_limit
 983             - Default mag_hold_rate_limit = 40dps and default MAG_HOLD P-gain is 40
 984             - With those values, maximum rotation speed will be required from Rate Controller when error is greater that 30 degrees
 985             - For smaller error, required rate will be proportional.
 986             - It uses LPF filter set at 2Hz to additionally smoothen out any rapid changes
 987             - That makes correction of smaller errors stronger, and those of big errors softer
 988
 989         This make looks as very slow rotation rate, but please remember this is automatic mode.
 990         Manual override with YAW input when MAG_HOLD is enabled will still use "manual" rates, not MAG_HOLD rates.
 991         Highest possible correction is 180 degrees and it will take more less 4.5 seconds. It is even more than sufficient
 992         to run RTH or WAYPOINT missions. My favourite rate range here is 20dps - 30dps that gives nice and smooth turns.
 993
 994         Correction for small errors is much faster now. For example, old contrioller for 2deg errors required 1dps (correction in 2 seconds).
 995         New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached.
 996     */
 997
 998     headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30.0f;
 999     headingHoldRate = constrainf(headingHoldRate, -pidProfile()->heading_hold_rate_limit, pidProfile()->heading_hold_rate_limit);
1000     headingHoldRate = pt1FilterApply4(&headingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT);
1001
1002     return headingHoldRate;
1003 }
1004
1005 /*
1006  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
1007  * and keeping ROLL and PITCH attitude though the turn.
1008  */
1009 static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget)
1010 {
1011     fpVector3_t targetRates;
1012     targetRates.x = 0.0f;
1013     targetRates.y = 0.0f;
1014
1015     if (STATE(AIRPLANE)) {
1016         if (calculateCosTiltAngle() >= 0.173648f) {
1017             // Ideal banked turn follow the equations:
1018             //      forward_vel^2 / radius = Gravity * tan(roll_angle)
1019             //      yaw_rate = forward_vel / radius
1020             // If we solve for roll angle we get:
1021             //      tan(roll_angle) = forward_vel * yaw_rate / Gravity
1022             // If we solve for yaw rate we get:
1023             //      yaw_rate = tan(roll_angle) * Gravity / forward_vel
1024
1025 #if defined(USE_PITOT)
1026             float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy()? getAirspeedEstimate() : pidProfile()->fixedWingReferenceAirspeed;
1027 #else
1028             float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed;
1029 #endif
1030
1031             // Constrain to somewhat sane limits - 10km/h - 216km/h
1032             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f);
1033
1034             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
1035             bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60));
1036             float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
1037             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
1038
1039             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
1040         }
1041         else {
1042             // Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive
1043             return;
1044         }
1045     }
1046     else {
1047         targetRates.z = pidState[YAW].rateTarget;
1048     }
1049
1050     // Transform calculated rate offsets into body frame and apply
1051     imuTransformVectorEarthToBody(&targetRates);
1052
1053     // Add in roll and pitch
1054     pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
1055     pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
1056
1057     // Replace YAW on quads - add it in on airplanes
1058     if (STATE(AIRPLANE)) {
1059         pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
1060     }
1061     else {
1062         pidState[YAW].rateTarget = constrainf(targetRates.z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
1063     }
1064 }
1065
1066 static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAngle)
1067 {
1068     static uint8_t lastFpvCamAngleDegrees = 0;
1069     static float cosCameraAngle = 1.0f;
1070     static float sinCameraAngle = 0.0f;
1071
1072     if (lastFpvCamAngleDegrees != fpvCameraAngle) {
1073         lastFpvCamAngleDegrees = fpvCameraAngle;
1074         cosCameraAngle = cos_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
1075         sinCameraAngle = sin_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
1076     }
1077
1078     // Rotate roll/yaw command from camera-frame coordinate system to body-frame coordinate system
1079     const float rollRate = pidState[ROLL].rateTarget;
1080     const float yawRate = pidState[YAW].rateTarget;
1081     pidState[ROLL].rateTarget = constrainf(rollRate * cosCameraAngle -  yawRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
1082     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
1083 }
1084
1085 void checkItermLimitingActive(pidState_t *pidState)
1086 {
1087     bool shouldActivate = false;
1088
1089     if (usedPidControllerType == PID_TYPE_PID) {
1090         shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
1091     }
1092
1093     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
1094 }
1095
1096 void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
1097 {
1098     if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
1099         // Do not allow yaw I-term to grow when bank angle is too large
1100         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
1101         if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
1102             pidState->itermFreezeActive = true;
1103         } else
1104         {
1105             pidState->itermFreezeActive = false;
1106         }
1107     } else
1108     {
1109         pidState->itermFreezeActive = false;
1110     }
1111
1112 }
1113
1114 bool isAngleHoldLevel(void)
1115 {
1116     return angleHoldIsLevel;
1117 }
1118
1119 void updateAngleHold(float *angleTarget, uint8_t axis)
1120 {
1121     int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
1122
1123     if (!restartAngleHoldMode) {     // set restart flag when anglehold is inactive
1124         restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1;
1125     }
1126
1127     if ((FLIGHT_MODE(ANGLEHOLD_MODE) || axis == navAngleHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
1128         /* angleHoldTarget stores attitude values using a zero datum when level.
1129          * This requires angleHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0
1130          * when the craft is level even though attitude pitch is non zero in this case.
1131          * angleTarget pitch is corrected back to fixedWingLevelTrim datum on return from function */
1132
1133         static int16_t angleHoldTarget[2];
1134
1135         if (restartAngleHoldMode) {      // set target attitude to current attitude on activation
1136             angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
1137             angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
1138             restartAngleHoldMode = false;
1139         }
1140
1141         // set flag indicating anglehold is level
1142         if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
1143             angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0;
1144         } else {
1145             angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0;
1146         }
1147
1148         uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
1149
1150         // use Nav bank angle limits if Nav active
1151         if (navAngleHoldAxis == FD_ROLL) {
1152             bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
1153         } else if (navAngleHoldAxis == FD_PITCH) {
1154             bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
1155         }
1156
1157         int16_t levelTrim = axis == FD_PITCH ? DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : 0;
1158         if (calculateRollPitchCenterStatus() == CENTERED) {
1159             angleHoldTarget[axis] = ABS(angleHoldTarget[axis]) < 30 ? 0 : angleHoldTarget[axis];   // snap to level when within 3 degs of level
1160             *angleTarget = constrain(angleHoldTarget[axis] - levelTrim, -bankLimit, bankLimit);
1161         } else {
1162             *angleTarget = constrain(attitude.raw[axis] + *angleTarget + levelTrim, -bankLimit, bankLimit);
1163             angleHoldTarget[axis] = attitude.raw[axis] + levelTrim;
1164         }
1165     }
1166 }
1167
1168 void FAST_CODE pidController(float dT)
1169 {
1170     const float dT_inv = 1.0f / dT;
1171
1172     if (!pidFiltersConfigured) {
1173         return;
1174     }
1175
1176     bool canUseFpvCameraMix = STATE(MULTIROTOR);
1177     uint8_t headingHoldState = getHeadingHoldState();
1178
1179     // In case Yaw override is active, we engage the Heading Hold state
1180     if (isFlightAxisAngleOverrideActive(FD_YAW)) {
1181         headingHoldState = HEADING_HOLD_ENABLED;
1182         headingHoldTarget = DECIDEGREES_TO_DEGREES(getFlightAxisAngleOverride(FD_YAW, 0));
1183     }
1184
1185     if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) {
1186         updateHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1187     }
1188
1189     for (int axis = 0; axis < 3; axis++) {
1190         pidState[axis].gyroRate = gyro.gyroADCf[axis];
1191
1192         // Step 2: Read target
1193         float rateTarget;
1194
1195         if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) {
1196             rateTarget = pidHeadingHold(dT);
1197         } else {
1198 #ifdef USE_PROGRAMMING_FRAMEWORK
1199             rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]);
1200 #else
1201             rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]);
1202 #endif
1203         }
1204
1205         // Limit desired rate to something gyro can measure reliably
1206         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
1207
1208 #ifdef USE_GYRO_KALMAN
1209         gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
1210 #endif
1211
1212 #ifdef USE_SMITH_PREDICTOR
1213         pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
1214 #endif
1215     }
1216
1217     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
1218     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
1219     angleHoldIsLevel = false;
1220
1221     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
1222         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
1223             // If axis angle override, get the correct angle from Logic Conditions
1224             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
1225
1226             //apply 45 deg offset for tailsitter when isMixerTransitionMixing is activated
1227             if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH){
1228                 angleTarget += DEGREES_TO_DECIDEGREES(45);
1229             }
1230
1231             if (STATE(AIRPLANE)) {  // update anglehold mode
1232                 updateAngleHold(&angleTarget, axis);
1233             }
1234
1235             // Apply the Level PID controller
1236             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
1237             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
1238         } else {
1239             restartAngleHoldMode = true;
1240         }
1241     }
1242
1243     // Apply Turn Assistance
1244     if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
1245         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
1246         float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
1247         pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
1248         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
1249     }
1250
1251     // Apply FPV camera mix
1252     if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) {
1253         pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
1254     }
1255
1256     // Prevent strong Iterm accumulation during stick inputs
1257     antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
1258
1259     for (int axis = 0; axis < 3; axis++) {
1260         // Apply setpoint rate of change limits
1261         pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
1262
1263         // Step 4: Run gyro-driven control
1264         checkItermLimitingActive(&pidState[axis]);
1265         checkItermFreezingActive(&pidState[axis], axis);
1266
1267         pidControllerApplyFn(&pidState[axis], dT, dT_inv);
1268     }
1269 }
1270
1271 pidType_e pidIndexGetType(pidIndex_e pidIndex)
1272 {
1273     if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
1274         return usedPidControllerType;
1275     }
1276     if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
1277         if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
1278             return PID_TYPE_NONE;
1279         }
1280     }
1281     // Common
1282     if (pidIndex == PID_SURFACE) {
1283         return PID_TYPE_NONE;
1284     }
1285     return PID_TYPE_PID;
1286 }
1287
1288 void pidInit(void)
1289 {
1290     // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
1291     headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
1292                            cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
1293
1294     pidGainsUpdateRequired = false;
1295
1296     itermRelax = pidProfile()->iterm_relax;
1297
1298     yawLpfHz = pidProfile()->yaw_lpf_hz;
1299     motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
1300
1301 #ifdef USE_D_BOOST
1302     dBoostMin = pidProfile()->dBoostMin;
1303     dBoostMax = pidProfile()->dBoostMax;
1304     dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
1305 #endif
1306
1307 #ifdef USE_ANTIGRAVITY
1308     antigravityGain = pidProfile()->antigravityGain;
1309     antigravityAccelerator = pidProfile()->antigravityAccelerator;
1310 #endif
1311
1312     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
1313
1314     #ifdef USE_D_BOOST
1315         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
1316         // We assume, max acceleration is when pilot deflects the stick fully in 100ms
1317         pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10;
1318     #endif
1319
1320         pidState[axis].axis = axis;
1321         if (axis == FD_YAW) {
1322             if (yawLpfHz) {
1323                 pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
1324             } else {
1325                 pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
1326             }
1327         } else {
1328             pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
1329         }
1330     }
1331
1332     if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
1333         if (
1334             currentMixerConfig.platformType == PLATFORM_AIRPLANE ||
1335             currentMixerConfig.platformType == PLATFORM_BOAT ||
1336             currentMixerConfig.platformType == PLATFORM_ROVER
1337         ) {
1338             usedPidControllerType = PID_TYPE_PIFF;
1339         } else {
1340             usedPidControllerType = PID_TYPE_PID;
1341         }
1342     } else {
1343         usedPidControllerType = pidProfile()->pidControllerType;
1344     }
1345
1346     assignFilterApplyFn(pidProfile()->dterm_lpf_type, pidProfile()->dterm_lpf_hz, &dTermLpfFilterApplyFn);
1347
1348     if (usedPidControllerType == PID_TYPE_PIFF) {
1349         pidControllerApplyFn = pidApplyFixedWingRateController;
1350     } else if (usedPidControllerType == PID_TYPE_PID) {
1351         pidControllerApplyFn = pidApplyMulticopterRateController;
1352     } else {
1353         pidControllerApplyFn = nullRateController;
1354     }
1355
1356     pidResetTPAFilter();
1357
1358     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
1359
1360     navPidInit(
1361         &fixedWingLevelTrimController,
1362         0.0f,
1363         (float)pidProfile()->fixedWingLevelTrimGain / 200.0f,
1364         0.0f,
1365         0.0f,
1366         2.0f,
1367         0.0f
1368     );
1369
1370 }
1371
1372 const pidBank_t * pidBank(void) {
1373     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
1374 }
1375
1376 pidBank_t * pidBankMutable(void) {
1377     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
1378 }
1379
1380 bool isFixedWingLevelTrimActive(void)
1381 {
1382     return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
1383            (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
1384            !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
1385            !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
1386 }
1387
1388 void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
1389 {
1390     if (!STATE(AIRPLANE)) {
1391         return;
1392     }
1393
1394     static bool previousArmingState = false;
1395
1396     if (ARMING_FLAG(ARMED)) {
1397         if (!previousArmingState) {     // On every ARM reset the controller
1398             navPidReset(&fixedWingLevelTrimController);
1399         }
1400     } else if (previousArmingState) {   // On disarm update the default value
1401         pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
1402     }
1403     previousArmingState = ARMING_FLAG(ARMED);
1404
1405     // return if not active or disarmed
1406     if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
1407         return;
1408     }
1409
1410     static timeUs_t previousUpdateTimeUs;
1411     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
1412     previousUpdateTimeUs = currentTimeUs;
1413
1414     /*
1415      * Prepare flags for the PID controller
1416      */
1417     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
1418
1419     // Iterm should freeze when conditions for setting level trim aren't met or time since last expected update too long ago
1420     if (!isFixedWingLevelTrimActive() || (dT > 5.0f * US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)))) {
1421         flags |= PID_FREEZE_INTEGRATOR;
1422     }
1423
1424     const float output = navPidApply3(
1425         &fixedWingLevelTrimController,
1426         0,  //Setpoint is always 0 as we try to keep level flight
1427         getEstimatedActualVelocity(Z),
1428         dT,
1429         -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
1430         FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
1431         flags,
1432         1.0f,
1433         1.0f
1434     );
1435
1436     DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
1437     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
1438 }
1439
1440 float getFixedWingLevelTrim(void)
1441 {
1442     return STATE(AIRPLANE) ? fixedWingLevelTrim : 0;
1443 }
1444
1445 uint16_t getPidSumLimit(const flight_dynamics_index_t axis) {
1446     if (axis == FD_YAW) {
1447         return STATE(MULTIROTOR) ? 400 : 500;
1448     } else {
1449         return 500;
1450     }
1451 }