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