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, 8);
 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 );
 316
 317 bool pidInitFilters(void)
 318 {
 319     const uint32_t refreshRate = getLooptime();
 320
 321     if (refreshRate == 0) {
 322         return false;
 323     }
 324
 325     for (int axis = 0; axis < 3; ++ axis) {
 326         initFilter(pidProfile()->dterm_lpf_type, &pidState[axis].dtermLpfState, pidProfile()->dterm_lpf_hz, refreshRate);
 327     }
 328
 329     for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
 330         pt1FilterInit(&windupLpf[i], pidProfile()->iterm_relax_cutoff, US2S(refreshRate));
 331     }
 332
 333 #ifdef USE_ANTIGRAVITY
 334     pt1FilterInit(&antigravityThrottleLpf, pidProfile()->antigravityCutoff, US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
 335 #endif
 336
 337 #ifdef USE_D_BOOST
 338     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 339         biquadFilterInitLPF(&pidState[axis].dBoostGyroLpf, pidProfile()->dBoostGyroDeltaLpfHz, getLooptime());
 340     }
 341 #endif
 342
 343     if (pidProfile()->controlDerivativeLpfHz) {
 344         for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 345             pt3FilterInit(&pidState[axis].rateTargetFilter, pt3FilterGain(pidProfile()->controlDerivativeLpfHz, US2S(refreshRate)));
 346         }
 347     }
 348
 349 #ifdef USE_SMITH_PREDICTOR
 350     smithPredictorInit(
 351         &pidState[FD_ROLL].smithPredictor,
 352         pidProfile()->smithPredictorDelay,
 353         pidProfile()->smithPredictorStrength,
 354         pidProfile()->smithPredictorFilterHz,
 355         getLooptime()
 356     );
 357     smithPredictorInit(
 358         &pidState[FD_PITCH].smithPredictor,
 359         pidProfile()->smithPredictorDelay,
 360         pidProfile()->smithPredictorStrength,
 361         pidProfile()->smithPredictorFilterHz,
 362         getLooptime()
 363     );
 364     smithPredictorInit(
 365         &pidState[FD_YAW].smithPredictor,
 366         pidProfile()->smithPredictorDelay,
 367         pidProfile()->smithPredictorStrength,
 368         pidProfile()->smithPredictorFilterHz,
 369         getLooptime()
 370     );
 371 #endif
 372
 373     pidFiltersConfigured = true;
 374
 375     return true;
 376 }
 377
 378 void pidResetTPAFilter(void)
 379 {
 380     if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
 381         pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlRateProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
 382         pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
 383     }
 384 }
 385
 386 void pidResetErrorAccumulators(void)
 387 {
 388     // Reset R/P/Y integrator
 389     for (int axis = 0; axis < 3; axis++) {
 390         pidState[axis].errorGyroIf = 0.0f;
 391         pidState[axis].errorGyroIfLimit = 0.0f;
 392     }
 393 }
 394
 395 void pidReduceErrorAccumulators(int8_t delta, uint8_t axis)
 396 {
 397     pidState[axis].errorGyroIf -= delta;
 398     pidState[axis].errorGyroIfLimit -= delta;
 399 }
 400
 401 float getTotalRateTarget(void)
 402 {
 403     return calc_length_pythagorean_3D(pidState[FD_ROLL].rateTarget, pidState[FD_PITCH].rateTarget, pidState[FD_YAW].rateTarget);
 404 }
 405
 406 float getAxisIterm(uint8_t axis)
 407 {
 408     return pidState[axis].errorGyroIf;
 409 }
 410
 411 static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination)
 412 {
 413     stick = constrain(stick, -500, 500);
 414     return scaleRangef((float) stick, -500.0f, 500.0f, (float) -maxInclination, (float) maxInclination);
 415 }
 416
 417 int16_t pidAngleToRcCommand(float angleDeciDegrees, int16_t maxInclination)
 418 {
 419     angleDeciDegrees = constrainf(angleDeciDegrees, (float) -maxInclination, (float) maxInclination);
 420     return scaleRangef((float) angleDeciDegrees, (float) -maxInclination, (float) maxInclination, -500.0f, 500.0f);
 421 }
 422
 423 /*
 424 Map stick positions to desired rotatrion rate in given axis.
 425 Rotation rate in dps at full stick deflection is defined by axis rate measured in dps/10
 426 Rate 20 means 200dps at full stick deflection
 427 */
 428 float pidRateToRcCommand(float rateDPS, uint8_t rate)
 429 {
 430     const float maxRateDPS = rate * 10.0f;
 431     return scaleRangef(rateDPS, -maxRateDPS, maxRateDPS, -500.0f, 500.0f);
 432 }
 433
 434 float pidRcCommandToRate(int16_t stick, uint8_t rate)
 435 {
 436     const float maxRateDPS = rate * 10.0f;
 437     return scaleRangef((float) stick, -500.0f, 500.0f, -maxRateDPS, maxRateDPS);
 438 }
 439
 440 static float calculateFixedWingTPAFactor(uint16_t throttle)
 441 {
 442     float tpaFactor;
 443
 444     // tpa_rate is amount of curve TPA applied to PIDs
 445     // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
 446     if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) {
 447         if (throttle > getThrottleIdleValue()) {
 448             // Calculate TPA according to throttle
 449             tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f);
 450
 451             // Limit to [0.5; 2] range
 452             tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
 453         }
 454         else {
 455             tpaFactor = 2.0f;
 456         }
 457
 458         // Attenuate TPA curve according to configured amount
 459         tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->throttle.dynPID / 100.0f);
 460     }
 461     else {
 462         tpaFactor = 1.0f;
 463     }
 464
 465     return tpaFactor;
 466 }
 467
 468 static float calculateMultirotorTPAFactor(void)
 469 {
 470     float tpaFactor;
 471
 472     // TPA should be updated only when TPA is actually set
 473     if (currentControlRateProfile->throttle.dynPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->throttle.pa_breakpoint) {
 474         tpaFactor = 1.0f;
 475     } else if (rcCommand[THROTTLE] < getMaxThrottle()) {
 476         tpaFactor = (100 - (uint16_t)currentControlRateProfile->throttle.dynPID * (rcCommand[THROTTLE] - currentControlRateProfile->throttle.pa_breakpoint) / (float)(getMaxThrottle() - currentControlRateProfile->throttle.pa_breakpoint)) / 100.0f;
 477     } else {
 478         tpaFactor = (100 - currentControlRateProfile->throttle.dynPID) / 100.0f;
 479     }
 480
 481     return tpaFactor;
 482 }
 483
 484 void schedulePidGainsUpdate(void)
 485 {
 486     pidGainsUpdateRequired = true;
 487 }
 488
 489 void updatePIDCoefficients(void)
 490 {
 491     STATIC_FASTRAM uint16_t prevThrottle = 0;
 492
 493     // Check if throttle changed. Different logic for fixed wing vs multirotor
 494     if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
 495         uint16_t filteredThrottle = pt1FilterApply(&fixedWingTpaFilter, rcCommand[THROTTLE]);
 496         if (filteredThrottle != prevThrottle) {
 497             prevThrottle = filteredThrottle;
 498             pidGainsUpdateRequired = true;
 499         }
 500     }
 501     else {
 502         if (rcCommand[THROTTLE] != prevThrottle) {
 503             prevThrottle = rcCommand[THROTTLE];
 504             pidGainsUpdateRequired = true;
 505         }
 506     }
 507
 508 #ifdef USE_ANTIGRAVITY
 509     if (usedPidControllerType == PID_TYPE_PID) {
 510         antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
 511         iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
 512     }
 513 #endif
 514
 515     /*
 516      * Compute stick position in range of [-1.0f : 1.0f] without deadband and expo
 517      */
 518     for (int axis = 0; axis < 3; axis++) {
 519         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
 520     }
 521
 522     // If nothing changed - don't waste time recalculating coefficients
 523     if (!pidGainsUpdateRequired) {
 524         return;
 525     }
 526
 527     const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
 528
 529     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
 530     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
 531     for (int axis = 0; axis < 3; axis++) {
 532         if (usedPidControllerType == PID_TYPE_PIFF) {
 533             // Airplanes - scale all PIDs according to TPA
 534             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER  * tpaFactor;
 535             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER  * tpaFactor;
 536             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * tpaFactor;
 537             pidState[axis].kFF = pidBank()->pid[axis].FF / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
 538             pidState[axis].kCD = 0.0f;
 539             pidState[axis].kT  = 0.0f;
 540         }
 541         else {
 542             const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor;
 543             pidState[axis].kP  = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
 544             pidState[axis].kI  = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
 545             pidState[axis].kD  = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
 546             pidState[axis].kCD = (pidBank()->pid[axis].FF / FP_PID_RATE_D_FF_MULTIPLIER * axisTPA) / (getLooptime() * 0.000001f);
 547             pidState[axis].kFF = 0.0f;
 548
 549             // Tracking anti-windup requires P/I/D to be all defined which is only true for MC
 550             if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0) && (usedPidControllerType == PID_TYPE_PID)) {
 551                 pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
 552             } else {
 553                 pidState[axis].kT = 0;
 554             }
 555         }
 556     }
 557
 558     pidGainsUpdateRequired = false;
 559 }
 560
 561 static float calcHorizonRateMagnitude(void)
 562 {
 563     // Figure out the raw stick positions
 564     const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL));
 565     const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH));
 566     const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f;
 567     const float modeTransitionStickPos = constrain(pidBank()->pid[PID_LEVEL].D, 0, 100) / 100.0f;
 568
 569     float horizonRateMagnitude;
 570
 571     // Calculate transition point according to stick deflection
 572     if (mostDeflectedStickPos <= modeTransitionStickPos) {
 573         horizonRateMagnitude = mostDeflectedStickPos / modeTransitionStickPos;
 574     }
 575     else {
 576         horizonRateMagnitude = 1.0f;
 577     }
 578
 579     return horizonRateMagnitude;
 580 }
 581
 582 /* ANGLE freefloat deadband (degs).Angle error only starts to increase if atttiude outside deadband. */
 583 int16_t angleFreefloatDeadband(int16_t deadband, flight_dynamics_index_t axis)
 584 {
 585     int16_t levelDatum = axis == FD_PITCH ? attitude.raw[axis] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : attitude.raw[axis];
 586     if (ABS(levelDatum) > deadband) {
 587         return levelDatum > 0 ? deadband - levelDatum : -(levelDatum + deadband);
 588     } else {
 589         return 0;
 590     }
 591 }
 592
 593 static float computePidLevelTarget(flight_dynamics_index_t axis) {
 594     // This is ROLL/PITCH, run ANGLE/HORIZON controllers
 595 #ifdef USE_PROGRAMMING_FRAMEWORK
 596     float angleTarget = pidRcCommandToAngle(getRcCommandOverride(rcCommand, axis), pidProfile()->max_angle_inclination[axis]);
 597 #else
 598     float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
 599 #endif
 600
 601     // Automatically pitch down if the throttle is manually controlled and reduced bellow cruise throttle
 602 #ifdef USE_FW_AUTOLAND
 603     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle() && !FLIGHT_MODE(NAV_FW_AUTOLAND)) {
 604 #else
 605     if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) {
 606 #endif
 607         angleTarget += scaleRange(MAX(0, currentBatteryProfile->nav.fw.cruise_throttle - rcCommand[THROTTLE]), 0, currentBatteryProfile->nav.fw.cruise_throttle - PWM_RANGE_MIN, 0, navConfig()->fw.minThrottleDownPitchAngle);
 608     }
 609
 610     //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming
 611     if (axis == FD_PITCH && STATE(AIRPLANE)) {
 612         DEBUG_SET(DEBUG_AUTOLEVEL, 0, angleTarget * 10);
 613         DEBUG_SET(DEBUG_AUTOLEVEL, 1, fixedWingLevelTrim * 10);
 614         DEBUG_SET(DEBUG_AUTOLEVEL, 2, getEstimatedActualVelocity(Z));
 615
 616         /*
 617          * fixedWingLevelTrim has opposite sign to rcCommand.
 618          * Positive rcCommand means nose should point downwards
 619          * Negative rcCommand mean nose should point upwards
 620          * This is counter intuitive and a natural way suggests that + should mean UP
 621          * This is why fixedWingLevelTrim has opposite sign to rcCommand
 622          * Positive fixedWingLevelTrim means nose should point upwards
 623          * Negative fixedWingLevelTrim means nose should point downwards
 624          */
 625         angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
 626         DEBUG_SET(DEBUG_AUTOLEVEL, 3, angleTarget * 10);
 627     }
 628
 629     return angleTarget;
 630 }
 631
 632 static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude, float dT)
 633 {
 634     float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);
 635
 636     // Soaring mode deadband inactive if pitch/roll stick not centered to allow RC stick adjustment
 637     if (FLIGHT_MODE(SOARING_MODE) && axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
 638         angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH));
 639         if (!angleErrorDeg) {
 640             pidState->errorGyroIf = 0.0f;
 641             pidState->errorGyroIfLimit = 0.0f;
 642         }
 643     }
 644
 645     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);
 646
 647     // Apply simple LPF to angleRateTarget to make response less jerky
 648     // Ideas behind this:
 649     //  1) Attitude is updated at gyro rate, rateTarget for ANGLE mode is calculated from attitude
 650     //  2) If this rateTarget is passed directly into gyro-base PID controller this effectively doubles the rateError.
 651     //     D-term that is calculated from error tend to amplify this even more. Moreover, this tend to respond to every
 652     //     slightest change in attitude making self-leveling jittery
 653     //  3) Lowering LEVEL P can make the effects of (2) less visible, but this also slows down self-leveling.
 654     //  4) Human pilot response to attitude change in RATE mode is fairly slow and smooth, human pilot doesn't
 655     //     compensate for each slightest change
 656     //  5) (2) and (4) lead to a simple idea of adding a low-pass filter on rateTarget for ANGLE mode damping
 657     //     response to rapid attitude changes and smoothing out self-leveling reaction
 658     if (pidBank()->pid[PID_LEVEL].I) {
 659         // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
 660         angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidBank()->pid[PID_LEVEL].I, dT);
 661     }
 662
 663     // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
 664     if (FLIGHT_MODE(HORIZON_MODE)) {
 665         pidState->rateTarget = (1.0f - horizonRateMagnitude) * angleRateTarget + horizonRateMagnitude * pidState->rateTarget;
 666     } else {
 667         pidState->rateTarget = angleRateTarget;
 668     }
 669 }
 670
 671 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
 672 static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
 673 {
 674     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 675
 676     if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) {
 677         pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT);
 678     }
 679 }
 680
 681 static float pTermProcess(pidState_t *pidState, float rateError, float dT) {
 682     float newPTerm = rateError * pidState->kP;
 683
 684     return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
 685 }
 686
 687 #ifdef USE_D_BOOST
 688 static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
 689
 690     float dBoost = 1.0f;
 691
 692     const float dBoostGyroDelta = (pidState->gyroRate - pidState->previousRateGyro) * dT_inv;
 693     const float dBoostGyroAcceleration = fabsf(biquadFilterApply(&pidState->dBoostGyroLpf, dBoostGyroDelta));
 694     const float dBoostRateAcceleration = fabsf((currentRateTarget - pidState->previousRateTarget) * dT_inv);
 695
 696     if (dBoostGyroAcceleration >= dBoostRateAcceleration) {
 697         //Gyro is accelerating faster than setpoint, we want to smooth out
 698         dBoost = scaleRangef(dBoostGyroAcceleration, 0.0f, dBoostMaxAtAlleceleration, 1.0f, dBoostMax);
 699     } else {
 700         //Setpoint is accelerating, we want to boost response
 701         dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, dBoostMin);
 702     }
 703
 704     dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
 705     dBoost = constrainf(dBoost, dBoostMin, dBoostMax);
 706
 707     return dBoost;
 708 }
 709 #else
 710 static float applyDBoost(pidState_t *pidState, float dT) {
 711     UNUSED(pidState);
 712     UNUSED(dT);
 713     return 1.0f;
 714 }
 715 #endif
 716
 717 static float dTermProcess(pidState_t *pidState, float currentRateTarget, float dT, float dT_inv) {
 718     // Calculate new D-term
 719     float newDTerm = 0;
 720     if (pidState->kD == 0) {
 721         // optimisation for when D is zero, often used by YAW axis
 722         newDTerm = 0;
 723     } else {
 724         float delta = pidState->previousRateGyro - pidState->gyroRate;
 725
 726         delta = dTermLpfFilterApplyFn((filter_t *) &pidState->dtermLpfState, delta);
 727
 728         // Calculate derivative
 729         newDTerm =  delta * (pidState->kD * dT_inv) * applyDBoost(pidState, currentRateTarget, dT, dT_inv);
 730     }
 731     return(newDTerm);
 732 }
 733
 734 static void applyItermLimiting(pidState_t *pidState) {
 735     if (pidState->itermLimitActive) {
 736         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
 737     } else
 738     {
 739         pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
 740     }
 741 }
 742
 743 static void nullRateController(pidState_t *pidState, float dT, float dT_inv) {
 744     UNUSED(pidState);
 745     UNUSED(dT);
 746     UNUSED(dT_inv);
 747 }
 748
 749 static void fwRateAttenuation(pidState_t *pidState, const float rateTarget, const float rateError) {
 750     const float maxRate = currentControlRateProfile->stabilized.rates[pidState->axis] * 10.0f;
 751     const float dampingFactor = attenuation(rateTarget, maxRate / 2.5f);
 752
 753     /*
 754      * Iterm damping is applied (down to 0) when:
 755      * abs(error) > 10% rate and sticks were moved in the last 500ms (hard stop at this mark)
 756
 757      * itermAttenuation  = MIN(curve(setpoint), (abs(error) > 10%) && (sticks were deflected in 500ms) ? 0 : 1)
 758      */
 759
 760     //If error is greater than 10% or max rate
 761     const bool errorThresholdReached = fabsf(rateError) > maxRate * 0.1f;
 762
 763     //If stick (setpoint) was moved above threshold in the last 500ms
 764     if (fabsf(rateTarget) > maxRate * 0.2f) {
 765         pidState->attenuation.targetOverThresholdTimeMs = millis();
 766     }
 767
 768     //If error is below threshold, we no longer track time for lock mechanism
 769     if (!errorThresholdReached) {
 770         pidState->attenuation.targetOverThresholdTimeMs = 0;
 771     }
 772
 773     pidState->attenuation.aI = MIN(dampingFactor, (errorThresholdReached && (millis() - pidState->attenuation.targetOverThresholdTimeMs) < 500) ? 0.0f : 1.0f);
 774
 775     //P & D damping factors are always the same and based on current damping factor
 776     pidState->attenuation.aP = dampingFactor;
 777     pidState->attenuation.aD = dampingFactor;
 778
 779     if (pidState->axis == FD_ROLL) {
 780         DEBUG_SET(DEBUG_ALWAYS, 0, pidState->attenuation.aP * 1000);
 781         DEBUG_SET(DEBUG_ALWAYS, 1, pidState->attenuation.aI * 1000);
 782         DEBUG_SET(DEBUG_ALWAYS, 2, pidState->attenuation.aD * 1000);
 783     }
 784 }
 785
 786 static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, float dT, float dT_inv)
 787 {
 788     const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 789
 790     const float rateError = rateTarget - pidState->gyroRate;
 791
 792     fwRateAttenuation(pidState, rateTarget, rateError);
 793
 794     const float newPTerm = pTermProcess(pidState, rateError, dT) * pidState->attenuation.aP;
 795     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv) * pidState->attenuation.aD;
 796     const float newFFTerm = rateTarget * pidState->kFF;
 797
 798     /*
 799      * Integral should be updated only if axis Iterm is not frozen
 800      */
 801     if (!pidState->itermFreezeActive) {
 802         pidState->errorGyroIf += rateError * pidState->kI * dT * pidState->attenuation.aI;
 803     }
 804
 805     applyItermLimiting(pidState);
 806
 807     const uint16_t limit = getPidSumLimit(pidState->axis);
 808
 809     if (pidProfile()->pidItermLimitPercent != 0){
 810         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
 811         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
 812     }
 813
 814     axisPID[pidState->axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -limit, +limit);
 815
 816     if (FLIGHT_MODE(SOARING_MODE) && pidState->axis == FD_PITCH && calculateRollPitchCenterStatus() == CENTERED) {
 817         if (!angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)) {
 818             axisPID[FD_PITCH] = 0;  // center pitch servo if pitch attitude within soaring mode deadband
 819         }
 820     }
 821
 822 #ifdef USE_AUTOTUNE_FIXED_WING
 823     if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) {
 824         autotuneFixedWingUpdate(pidState->axis, rateTarget, pidState->gyroRate, constrainf(newPTerm + newFFTerm, -limit, +limit));
 825     }
 826 #endif
 827
 828 #ifdef USE_BLACKBOX
 829     axisPID_P[pidState->axis] = newPTerm;
 830     axisPID_I[pidState->axis] = pidState->errorGyroIf;
 831     axisPID_D[pidState->axis] = newDTerm;
 832     axisPID_F[pidState->axis] = newFFTerm;
 833     axisPID_Setpoint[pidState->axis] = rateTarget;
 834 #endif
 835
 836     pidState->previousRateGyro = pidState->gyroRate;
 837
 838 }
 839
 840 static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, float itermErrorRate)
 841 {
 842     if (itermRelax) {
 843         if (axis < FD_YAW || itermRelax == ITERM_RELAX_RPY) {
 844
 845             const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint);
 846             const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf);
 847
 848             const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD);
 849             return itermErrorRate * itermRelaxFactor;
 850         }
 851     }
 852
 853     return itermErrorRate;
 854 }
 855
 856 static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, float dT, float dT_inv)
 857 {
 858
 859     const float rateTarget = getFlightAxisRateOverride(pidState->axis, pidState->rateTarget);
 860
 861     const float rateError = rateTarget - pidState->gyroRate;
 862     const float newPTerm = pTermProcess(pidState, rateError, dT);
 863     const float newDTerm = dTermProcess(pidState, rateTarget, dT, dT_inv);
 864
 865     const float rateTargetDelta = rateTarget - pidState->previousRateTarget;
 866     const float rateTargetDeltaFiltered = pt3FilterApply(&pidState->rateTargetFilter, rateTargetDelta);
 867
 868     /*
 869      * Compute Control Derivative
 870      */
 871     const float newCDTerm = rateTargetDeltaFiltered * pidState->kCD;
 872
 873     const uint16_t limit = getPidSumLimit(pidState->axis);
 874
 875     // TODO: Get feedback from mixer on available correction range for each axis
 876     const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm;
 877     const float newOutputLimited = constrainf(newOutput, -limit, +limit);
 878
 879     float itermErrorRate = applyItermRelax(pidState->axis, rateTarget, rateError);
 880
 881 #ifdef USE_ANTIGRAVITY
 882     itermErrorRate *= iTermAntigravityGain;
 883 #endif
 884
 885     pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT)
 886                              + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
 887
 888     if (pidProfile()->pidItermLimitPercent != 0){
 889         float itermLimit = limit * pidProfile()->pidItermLimitPercent * 0.01f;
 890         pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit);
 891     }
 892
 893
 894     // Don't grow I-term if motors are at their limit
 895     applyItermLimiting(pidState);
 896
 897     axisPID[pidState->axis] = newOutputLimited;
 898
 899 #ifdef USE_BLACKBOX
 900     axisPID_P[pidState->axis] = newPTerm;
 901     axisPID_I[pidState->axis] = pidState->errorGyroIf;
 902     axisPID_D[pidState->axis] = newDTerm;
 903     axisPID_F[pidState->axis] = newCDTerm;
 904     axisPID_Setpoint[pidState->axis] = rateTarget;
 905 #endif
 906
 907     pidState->previousRateTarget = rateTarget;
 908     pidState->previousRateGyro = pidState->gyroRate;
 909 }
 910
 911 void updateHeadingHoldTarget(int16_t heading)
 912 {
 913     headingHoldTarget = heading;
 914 }
 915
 916 void resetHeadingHoldTarget(int16_t heading)
 917 {
 918     updateHeadingHoldTarget(heading);
 919     pt1FilterReset(&headingHoldRateFilter, 0.0f);
 920 }
 921
 922 int16_t getHeadingHoldTarget(void) {
 923     return headingHoldTarget;
 924 }
 925
 926 static uint8_t getHeadingHoldState(void)
 927 {
 928     // Don't apply heading hold if overall tilt is greater than maximum angle inclination
 929     if (calculateCosTiltAngle() < headingHoldCosZLimit) {
 930         return HEADING_HOLD_DISABLED;
 931     }
 932
 933     int navHeadingState = navigationGetHeadingControlState();
 934     // NAV will prevent MAG_MODE from activating, but require heading control
 935     if (navHeadingState != NAV_HEADING_CONTROL_NONE) {
 936         // Apply maghold only if heading control is in auto mode
 937         if (navHeadingState == NAV_HEADING_CONTROL_AUTO) {
 938             return HEADING_HOLD_ENABLED;
 939         }
 940     }
 941     else if (ABS(rcCommand[YAW]) == 0 && FLIGHT_MODE(HEADING_MODE)) {
 942         return HEADING_HOLD_ENABLED;
 943     }
 944
 945     return HEADING_HOLD_UPDATE_HEADING;
 946 }
 947
 948 /*
 949  * HEADING_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
 950  */
 951 float pidHeadingHold(float dT)
 952 {
 953     float headingHoldRate;
 954
 955     /* Convert absolute error into relative to current heading */
 956     int16_t error = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headingHoldTarget;
 957
 958     /* Convert absolute error into relative to current heading */
 959     if (error > 180) {
 960         error -= 360;
 961     } else if (error < -180) {
 962         error += 360;
 963     }
 964
 965     /*
 966         New MAG_HOLD controller work slightly different that previous one.
 967         Old one mapped error to rotation speed in following way:
 968             - on rate 0 it gave about 0.5dps for each degree of error
 969             - error 0 = rotation speed of 0dps
 970             - error 180 = rotation speed of 96 degrees per second
 971             - output
 972             - that gives about 2 seconds to correct any error, no matter how big. Of course, usually more because of inertia.
 973         That was making him quite "soft" for small changes and rapid for big ones that started to appear
 974         when INAV introduced real RTH and WAYPOINT that might require rapid heading changes.
 975
 976         New approach uses modified principle:
 977             - manual yaw rate is not used. MAG_HOLD is decoupled from manual input settings
 978             - instead, mag_hold_rate_limit is used. It defines max rotation speed in dps that MAG_HOLD controller can require from RateController
 979             - computed rotation speed is capped at -mag_hold_rate_limit and mag_hold_rate_limit
 980             - Default mag_hold_rate_limit = 40dps and default MAG_HOLD P-gain is 40
 981             - With those values, maximum rotation speed will be required from Rate Controller when error is greater that 30 degrees
 982             - For smaller error, required rate will be proportional.
 983             - It uses LPF filter set at 2Hz to additionally smoothen out any rapid changes
 984             - That makes correction of smaller errors stronger, and those of big errors softer
 985
 986         This make looks as very slow rotation rate, but please remember this is automatic mode.
 987         Manual override with YAW input when MAG_HOLD is enabled will still use "manual" rates, not MAG_HOLD rates.
 988         Highest possible correction is 180 degrees and it will take more less 4.5 seconds. It is even more than sufficient
 989         to run RTH or WAYPOINT missions. My favourite rate range here is 20dps - 30dps that gives nice and smooth turns.
 990
 991         Correction for small errors is much faster now. For example, old contrioller for 2deg errors required 1dps (correction in 2 seconds).
 992         New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached.
 993     */
 994
 995     headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30.0f;
 996     headingHoldRate = constrainf(headingHoldRate, -pidProfile()->heading_hold_rate_limit, pidProfile()->heading_hold_rate_limit);
 997     headingHoldRate = pt1FilterApply4(&headingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT);
 998
 999     return headingHoldRate;
1000 }
1001
1002 /*
1003  * TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
1004  * and keeping ROLL and PITCH attitude though the turn.
1005  */
1006 static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget)
1007 {
1008     fpVector3_t targetRates;
1009     targetRates.x = 0.0f;
1010     targetRates.y = 0.0f;
1011
1012     if (STATE(AIRPLANE)) {
1013         if (calculateCosTiltAngle() >= 0.173648f) {
1014             // Ideal banked turn follow the equations:
1015             //      forward_vel^2 / radius = Gravity * tan(roll_angle)
1016             //      yaw_rate = forward_vel / radius
1017             // If we solve for roll angle we get:
1018             //      tan(roll_angle) = forward_vel * yaw_rate / Gravity
1019             // If we solve for yaw rate we get:
1020             //      yaw_rate = tan(roll_angle) * Gravity / forward_vel
1021
1022 #if defined(USE_PITOT)
1023             float airspeedForCoordinatedTurn = sensors(SENSOR_PITOT) && pitotIsHealthy()? getAirspeedEstimate() : pidProfile()->fixedWingReferenceAirspeed;
1024 #else
1025             float airspeedForCoordinatedTurn = pidProfile()->fixedWingReferenceAirspeed;
1026 #endif
1027
1028             // Constrain to somewhat sane limits - 10km/h - 216km/h
1029             airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300.0f, 6000.0f);
1030
1031             // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge
1032             bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60));
1033             float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
1034             float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
1035
1036             targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
1037         }
1038         else {
1039             // Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive
1040             return;
1041         }
1042     }
1043     else {
1044         targetRates.z = pidState[YAW].rateTarget;
1045     }
1046
1047     // Transform calculated rate offsets into body frame and apply
1048     imuTransformVectorEarthToBody(&targetRates);
1049
1050     // Add in roll and pitch
1051     pidState[ROLL].rateTarget = constrainf(pidState[ROLL].rateTarget + targetRates.x, -currentControlRateProfile->stabilized.rates[ROLL] * 10.0f, currentControlRateProfile->stabilized.rates[ROLL] * 10.0f);
1052     pidState[PITCH].rateTarget = constrainf(pidState[PITCH].rateTarget + targetRates.y * pidProfile()->fixedWingCoordinatedPitchGain, -currentControlRateProfile->stabilized.rates[PITCH] * 10.0f, currentControlRateProfile->stabilized.rates[PITCH] * 10.0f);
1053
1054     // Replace YAW on quads - add it in on airplanes
1055     if (STATE(AIRPLANE)) {
1056         pidState[YAW].rateTarget = constrainf(pidState[YAW].rateTarget + targetRates.z * pidProfile()->fixedWingCoordinatedYawGain, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
1057     }
1058     else {
1059         pidState[YAW].rateTarget = constrainf(targetRates.z, -currentControlRateProfile->stabilized.rates[YAW] * 10.0f, currentControlRateProfile->stabilized.rates[YAW] * 10.0f);
1060     }
1061 }
1062
1063 static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAngle)
1064 {
1065     static uint8_t lastFpvCamAngleDegrees = 0;
1066     static float cosCameraAngle = 1.0f;
1067     static float sinCameraAngle = 0.0f;
1068
1069     if (lastFpvCamAngleDegrees != fpvCameraAngle) {
1070         lastFpvCamAngleDegrees = fpvCameraAngle;
1071         cosCameraAngle = cos_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
1072         sinCameraAngle = sin_approx(DEGREES_TO_RADIANS(fpvCameraAngle));
1073     }
1074
1075     // Rotate roll/yaw command from camera-frame coordinate system to body-frame coordinate system
1076     const float rollRate = pidState[ROLL].rateTarget;
1077     const float yawRate = pidState[YAW].rateTarget;
1078     pidState[ROLL].rateTarget = constrainf(rollRate * cosCameraAngle -  yawRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
1079     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
1080 }
1081
1082 void checkItermLimitingActive(pidState_t *pidState)
1083 {
1084     bool shouldActivate = false;
1085
1086     if (usedPidControllerType == PID_TYPE_PID) {
1087         shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent
1088     }
1089
1090     pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
1091 }
1092
1093 void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis)
1094 {
1095     if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) {
1096         // Do not allow yaw I-term to grow when bank angle is too large
1097         float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll);
1098         if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){
1099             pidState->itermFreezeActive = true;
1100         } else
1101         {
1102             pidState->itermFreezeActive = false;
1103         }
1104     } else
1105     {
1106         pidState->itermFreezeActive = false;
1107     }
1108
1109 }
1110
1111 bool isAngleHoldLevel(void)
1112 {
1113     return angleHoldIsLevel;
1114 }
1115
1116 void updateAngleHold(float *angleTarget, uint8_t axis)
1117 {
1118     int8_t navAngleHoldAxis = navCheckActiveAngleHoldAxis();
1119
1120     if (!restartAngleHoldMode) {     // set restart flag when anglehold is inactive
1121         restartAngleHoldMode = !FLIGHT_MODE(ANGLEHOLD_MODE) && navAngleHoldAxis == -1;
1122     }
1123
1124     if ((FLIGHT_MODE(ANGLEHOLD_MODE) || axis == navAngleHoldAxis) && !isFlightAxisAngleOverrideActive(axis)) {
1125         /* angleHoldTarget stores attitude values using a zero datum when level.
1126          * This requires angleHoldTarget pitch to be corrected for fixedWingLevelTrim so it is 0
1127          * when the craft is level even though attitude pitch is non zero in this case.
1128          * angleTarget pitch is corrected back to fixedWingLevelTrim datum on return from function */
1129
1130         static int16_t angleHoldTarget[2];
1131
1132         if (restartAngleHoldMode) {      // set target attitude to current attitude on activation
1133             angleHoldTarget[FD_ROLL] = attitude.raw[FD_ROLL];
1134             angleHoldTarget[FD_PITCH] = attitude.raw[FD_PITCH] + DEGREES_TO_DECIDEGREES(fixedWingLevelTrim);
1135             restartAngleHoldMode = false;
1136         }
1137
1138         // set flag indicating anglehold is level
1139         if (FLIGHT_MODE(ANGLEHOLD_MODE)) {
1140             angleHoldIsLevel = angleHoldTarget[FD_ROLL] == 0 && angleHoldTarget[FD_PITCH] == 0;
1141         } else {
1142             angleHoldIsLevel = angleHoldTarget[navAngleHoldAxis] == 0;
1143         }
1144
1145         uint16_t bankLimit = pidProfile()->max_angle_inclination[axis];
1146
1147         // use Nav bank angle limits if Nav active
1148         if (navAngleHoldAxis == FD_ROLL) {
1149             bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle);
1150         } else if (navAngleHoldAxis == FD_PITCH) {
1151             bankLimit = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
1152         }
1153
1154         int16_t levelTrim = axis == FD_PITCH ? DEGREES_TO_DECIDEGREES(fixedWingLevelTrim) : 0;
1155         if (calculateRollPitchCenterStatus() == CENTERED) {
1156             angleHoldTarget[axis] = ABS(angleHoldTarget[axis]) < 30 ? 0 : angleHoldTarget[axis];   // snap to level when within 3 degs of level
1157             *angleTarget = constrain(angleHoldTarget[axis] - levelTrim, -bankLimit, bankLimit);
1158         } else {
1159             *angleTarget = constrain(attitude.raw[axis] + *angleTarget + levelTrim, -bankLimit, bankLimit);
1160             angleHoldTarget[axis] = attitude.raw[axis] + levelTrim;
1161         }
1162     }
1163 }
1164
1165 void FAST_CODE pidController(float dT)
1166 {
1167     const float dT_inv = 1.0f / dT;
1168
1169     if (!pidFiltersConfigured) {
1170         return;
1171     }
1172
1173     bool canUseFpvCameraMix = STATE(MULTIROTOR);
1174     uint8_t headingHoldState = getHeadingHoldState();
1175
1176     // In case Yaw override is active, we engage the Heading Hold state
1177     if (isFlightAxisAngleOverrideActive(FD_YAW)) {
1178         headingHoldState = HEADING_HOLD_ENABLED;
1179         headingHoldTarget = DECIDEGREES_TO_DEGREES(getFlightAxisAngleOverride(FD_YAW, 0));
1180     }
1181
1182     if (headingHoldState == HEADING_HOLD_UPDATE_HEADING) {
1183         updateHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw));
1184     }
1185
1186     for (int axis = 0; axis < 3; axis++) {
1187         pidState[axis].gyroRate = gyro.gyroADCf[axis];
1188
1189         // Step 2: Read target
1190         float rateTarget;
1191
1192         if (axis == FD_YAW && headingHoldState == HEADING_HOLD_ENABLED) {
1193             rateTarget = pidHeadingHold(dT);
1194         } else {
1195 #ifdef USE_PROGRAMMING_FRAMEWORK
1196             rateTarget = pidRcCommandToRate(getRcCommandOverride(rcCommand, axis), currentControlRateProfile->stabilized.rates[axis]);
1197 #else
1198             rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->stabilized.rates[axis]);
1199 #endif
1200         }
1201
1202         // Limit desired rate to something gyro can measure reliably
1203         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
1204
1205 #ifdef USE_GYRO_KALMAN
1206         gyroKalmanUpdateSetpoint(axis, pidState[axis].rateTarget);
1207 #endif
1208
1209 #ifdef USE_SMITH_PREDICTOR
1210         pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
1211 #endif
1212     }
1213
1214     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE and ANGLEHOLD_MODE
1215     const float horizonRateMagnitude = FLIGHT_MODE(HORIZON_MODE) ? calcHorizonRateMagnitude() : 0.0f;
1216     angleHoldIsLevel = false;
1217
1218     for (uint8_t axis = FD_ROLL; axis <= FD_PITCH; axis++) {
1219         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || FLIGHT_MODE(ANGLEHOLD_MODE) || isFlightAxisAngleOverrideActive(axis)) {
1220             // If axis angle override, get the correct angle from Logic Conditions
1221             float angleTarget = getFlightAxisAngleOverride(axis, computePidLevelTarget(axis));
1222
1223             //apply 45 deg offset for tailsitter when isMixerTransitionMixing is activated
1224             if (STATE(TAILSITTER) && isMixerTransitionMixing && axis == FD_PITCH){
1225                 angleTarget += DEGREES_TO_DECIDEGREES(45);
1226             }
1227
1228             if (STATE(AIRPLANE)) {  // update anglehold mode
1229                 updateAngleHold(&angleTarget, axis);
1230             }
1231
1232             // Apply the Level PID controller
1233             pidLevel(angleTarget, &pidState[axis], axis, horizonRateMagnitude, dT);
1234             canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with ANGLE/HORIZON
1235         } else {
1236             restartAngleHoldMode = true;
1237         }
1238     }
1239
1240     // Apply Turn Assistance
1241     if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
1242         float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
1243         float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
1244         pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
1245         canUseFpvCameraMix = false;     // FPVANGLEMIX is incompatible with TURN_ASSISTANT
1246     }
1247
1248     // Apply FPV camera mix
1249     if (canUseFpvCameraMix && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && currentControlRateProfile->misc.fpvCamAngleDegrees && STATE(MULTIROTOR)) {
1250         pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
1251     }
1252
1253     // Prevent strong Iterm accumulation during stick inputs
1254     antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f);
1255
1256     for (int axis = 0; axis < 3; axis++) {
1257         // Apply setpoint rate of change limits
1258         pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
1259
1260         // Step 4: Run gyro-driven control
1261         checkItermLimitingActive(&pidState[axis]);
1262         checkItermFreezingActive(&pidState[axis], axis);
1263
1264         pidControllerApplyFn(&pidState[axis], dT, dT_inv);
1265     }
1266 }
1267
1268 pidType_e pidIndexGetType(pidIndex_e pidIndex)
1269 {
1270     if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
1271         return usedPidControllerType;
1272     }
1273     if (STATE(AIRPLANE) || STATE(ROVER) || STATE(BOAT)) {
1274         if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
1275             return PID_TYPE_NONE;
1276         }
1277     }
1278     // Common
1279     if (pidIndex == PID_SURFACE) {
1280         return PID_TYPE_NONE;
1281     }
1282     return PID_TYPE_PID;
1283 }
1284
1285 void pidInit(void)
1286 {
1287     // Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
1288     headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
1289                            cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
1290
1291     pidGainsUpdateRequired = false;
1292
1293     itermRelax = pidProfile()->iterm_relax;
1294
1295     yawLpfHz = pidProfile()->yaw_lpf_hz;
1296     motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f));
1297
1298 #ifdef USE_D_BOOST
1299     dBoostMin = pidProfile()->dBoostMin;
1300     dBoostMax = pidProfile()->dBoostMax;
1301     dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
1302 #endif
1303
1304 #ifdef USE_ANTIGRAVITY
1305     antigravityGain = pidProfile()->antigravityGain;
1306     antigravityAccelerator = pidProfile()->antigravityAccelerator;
1307 #endif
1308
1309     for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
1310
1311     #ifdef USE_D_BOOST
1312         // Rate * 10 * 10. First 10 is to convert stick to DPS. Second 10 is to convert target to acceleration.
1313         // We assume, max acceleration is when pilot deflects the stick fully in 100ms
1314         pidState[axis].dBoostTargetAcceleration = currentControlRateProfile->stabilized.rates[axis] * 10 * 10;
1315     #endif
1316
1317         pidState[axis].axis = axis;
1318         if (axis == FD_YAW) {
1319             if (yawLpfHz) {
1320                 pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
1321             } else {
1322                 pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
1323             }
1324         } else {
1325             pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
1326         }
1327     }
1328
1329     if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
1330         if (
1331             currentMixerConfig.platformType == PLATFORM_AIRPLANE ||
1332             currentMixerConfig.platformType == PLATFORM_BOAT ||
1333             currentMixerConfig.platformType == PLATFORM_ROVER
1334         ) {
1335             usedPidControllerType = PID_TYPE_PIFF;
1336         } else {
1337             usedPidControllerType = PID_TYPE_PID;
1338         }
1339     } else {
1340         usedPidControllerType = pidProfile()->pidControllerType;
1341     }
1342
1343     assignFilterApplyFn(pidProfile()->dterm_lpf_type, pidProfile()->dterm_lpf_hz, &dTermLpfFilterApplyFn);
1344
1345     if (usedPidControllerType == PID_TYPE_PIFF) {
1346         pidControllerApplyFn = pidApplyFixedWingRateController;
1347     } else if (usedPidControllerType == PID_TYPE_PID) {
1348         pidControllerApplyFn = pidApplyMulticopterRateController;
1349     } else {
1350         pidControllerApplyFn = nullRateController;
1351     }
1352
1353     pidResetTPAFilter();
1354
1355     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim;
1356
1357     navPidInit(
1358         &fixedWingLevelTrimController,
1359         0.0f,
1360         (float)pidProfile()->fixedWingLevelTrimGain / 200.0f,
1361         0.0f,
1362         0.0f,
1363         2.0f,
1364         0.0f
1365     );
1366
1367 }
1368
1369 const pidBank_t * pidBank(void) {
1370     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc;
1371 }
1372
1373 pidBank_t * pidBankMutable(void) {
1374     return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc;
1375 }
1376
1377 bool isFixedWingLevelTrimActive(void)
1378 {
1379     return IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) && !areSticksDeflected() &&
1380            (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) &&
1381            !FLIGHT_MODE(SOARING_MODE) && !FLIGHT_MODE(MANUAL_MODE) &&
1382            !navigationIsControllingAltitude() && !(navCheckActiveAngleHoldAxis() == FD_PITCH && !angleHoldIsLevel);
1383 }
1384
1385 void updateFixedWingLevelTrim(timeUs_t currentTimeUs)
1386 {
1387     if (!STATE(AIRPLANE)) {
1388         return;
1389     }
1390
1391     static bool previousArmingState = false;
1392
1393     if (ARMING_FLAG(ARMED)) {
1394         if (!previousArmingState) {     // On every ARM reset the controller
1395             navPidReset(&fixedWingLevelTrimController);
1396         }
1397     } else if (previousArmingState) {   // On disarm update the default value
1398         pidProfileMutable()->fixedWingLevelTrim = constrainf(fixedWingLevelTrim, -FIXED_WING_LEVEL_TRIM_MAX_ANGLE, FIXED_WING_LEVEL_TRIM_MAX_ANGLE);
1399     }
1400     previousArmingState = ARMING_FLAG(ARMED);
1401
1402     // return if not active or disarmed
1403     if (!IS_RC_MODE_ACTIVE(BOXAUTOLEVEL) || !ARMING_FLAG(ARMED)) {
1404         return;
1405     }
1406
1407     static timeUs_t previousUpdateTimeUs;
1408     const float dT = US2S(currentTimeUs - previousUpdateTimeUs);
1409     previousUpdateTimeUs = currentTimeUs;
1410
1411     /*
1412      * Prepare flags for the PID controller
1413      */
1414     pidControllerFlags_e flags = PID_LIMIT_INTEGRATOR;
1415
1416     // Iterm should freeze when conditions for setting level trim aren't met or time since last expected update too long ago
1417     if (!isFixedWingLevelTrimActive() || (dT > 5.0f * US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)))) {
1418         flags |= PID_FREEZE_INTEGRATOR;
1419     }
1420
1421     const float output = navPidApply3(
1422         &fixedWingLevelTrimController,
1423         0,  //Setpoint is always 0 as we try to keep level flight
1424         getEstimatedActualVelocity(Z),
1425         dT,
1426         -FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
1427         FIXED_WING_LEVEL_TRIM_CONTROLLER_LIMIT,
1428         flags,
1429         1.0f,
1430         1.0f
1431     );
1432
1433     DEBUG_SET(DEBUG_AUTOLEVEL, 4, output);
1434     fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim + (output * FIXED_WING_LEVEL_TRIM_MULTIPLIER);
1435 }
1436
1437 float getFixedWingLevelTrim(void)
1438 {
1439     return STATE(AIRPLANE) ? fixedWingLevelTrim : 0;
1440 }
1441
1442 uint16_t getPidSumLimit(const flight_dynamics_index_t axis) {
1443     if (axis == FD_YAW) {
1444         return STATE(MULTIROTOR) ? 400 : 500;
1445     } else {
1446         return 500;
1447     }
1448 }