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