src/main/flight/pid_mwrewrite.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 #define SRC_MAIN_FLIGHT_PID_MWREWRITE_C_
  19
  20 #include <stdbool.h>
  21 #include <stdint.h>
  22 #include <string.h>
  23 #include <math.h>
  24
  25 #include <platform.h>
  26
  27 #include "build/build_config.h"
  28
  29 #ifdef USE_PID_MWREWRITE
  30
  31 #include "common/axis.h"
  32 #include "common/maths.h"
  33 #include "common/filter.h"
  34
  35 #include "config/parameter_group.h"
  36
  37 #include "drivers/sensor.h"
  38 #include "drivers/accgyro.h"
  39 #include "drivers/gyro_sync.h"
  40
  41 #include "sensors/sensors.h"
  42 #include "sensors/gyro.h"
  43 #include "sensors/acceleration.h"
  44
  45 #include "rx/rx.h"
  46
  47 #include "fc/rc_controls.h"
  48 #include "fc/rate_profile.h"
  49 #include "fc/runtime_config.h"
  50
  51 #include "flight/pid.h"
  52 #include "config/config_unittest.h"
  53 #include "flight/imu.h"
  54 #include "flight/navigation.h"
  55 #include "flight/gtune.h"
  56 #include "flight/mixer.h"
  57
  58 extern uint8_t PIDweight[3];
  59 extern int32_t lastITerm[3], ITermLimit[3];
  60
  61 extern pt1Filter_t deltaFilter[3];
  62 extern pt1Filter_t yawFilter;
  63
  64 extern uint8_t motorCount;
  65
  66 #ifdef BLACKBOX
  67 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  68 #endif
  69
  70 STATIC_UNIT_TESTED int16_t pidMultiWiiRewriteCore(int axis, const pidProfile_t *pidProfile, int32_t gyroRate, int32_t angleRate)
  71 {
  72     static int32_t lastRateForDelta[3];
  73
  74     SET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis);
  75
  76     const int32_t rateError = angleRate - gyroRate;
  77
  78     // -----calculate P component
  79     int32_t PTerm = (rateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
  80     // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply
  81     if (axis == YAW) {
  82         if (pidProfile->yaw_lpf_hz) {
  83             PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
  84         }
  85         if (pidProfile->yaw_p_limit && motorCount >= 4) {
  86             PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
  87         }
  88     }
  89
  90     // -----calculate I component
  91     // There should be no division before accumulating the error to integrator, because the precision would be reduced.
  92     // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator (Q19.13 format) is used.
  93     // Time correction (to avoid different I scaling for different builds based on average cycle time)
  94     // is normalized to cycle time = 2048 (2^11).
  95     int32_t ITerm = lastITerm[axis] + ((rateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis];
  96     // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
  97     // I coefficient (I8) moved before integration to make limiting independent from PID settings
  98     ITerm = constrain(ITerm, (int32_t)(-PID_MAX_I << 13), (int32_t)(PID_MAX_I << 13));
  99     // Anti windup protection
 100     if (rcModeIsActive(BOXAIRMODE)) {
 101         if (STATE(ANTI_WINDUP) || motorLimitReached) {
 102             ITerm = constrain(ITerm, -ITermLimit[axis], ITermLimit[axis]);
 103         } else {
 104             ITermLimit[axis] = ABS(ITerm);
 105         }
 106     }
 107     lastITerm[axis] = ITerm;
 108     ITerm = ITerm >> 13; // take integer part of Q19.13 value
 109
 110     // -----calculate D component
 111     int32_t DTerm;
 112     if (pidProfile->D8[axis] == 0) {
 113         // optimisation for when D8 is zero, often used by YAW axis
 114         DTerm = 0;
 115     } else {
 116         int32_t delta;
 117         if (pidProfile->deltaMethod == PID_DELTA_FROM_MEASUREMENT) {
 118             delta = -(gyroRate - lastRateForDelta[axis]);
 119             lastRateForDelta[axis] = gyroRate;
 120         } else {
 121             delta = rateError - lastRateForDelta[axis];
 122             lastRateForDelta[axis] = rateError;
 123         }
 124         // Divide delta by targetLooptime to get differential (ie dr/dt)
 125         delta = (delta * ((uint16_t)0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 5;
 126         if (pidProfile->dterm_lpf_hz) {
 127             // DTerm delta low pass filter
 128             delta = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)delta, pidProfile->dterm_lpf_hz, getdT()));
 129         }
 130         DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 131         DTerm = constrain(DTerm, -PID_MAX_D, PID_MAX_D);
 132     }
 133
 134 #ifdef BLACKBOX
 135     axisPID_P[axis] = PTerm;
 136     axisPID_I[axis] = ITerm;
 137     axisPID_D[axis] = DTerm;
 138 #endif
 139     GET_PID_MULTI_WII_REWRITE_CORE_LOCALS(axis);
 140     // -----calculate total PID output
 141     return PTerm + ITerm + DTerm;
 142 }
 143
 144 void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
 145         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
 146 {
 147
 148     pidInitFilters(pidProfile);
 149
 150     int horizonLevelStrength = 0;
 151     if (FLIGHT_MODE(HORIZON_MODE)) {
 152         // Using Level D as a Sensitivity for Horizon. 0 more rate to 255 more level.
 153         // For more rate mode decrease D and slower flips and rolls will be possible
 154         horizonLevelStrength = calcHorizonLevelStrength(rxConfig->midrc, pidProfile->horizon_tilt_effect,
 155                 pidProfile->horizon_tilt_mode, pidProfile->D8[PIDLEVEL]);
 156     }
 157
 158     // ----------PID controller----------
 159     for (int axis = 0; axis < 3; axis++) {
 160         const uint8_t rate = controlRateConfig->rates[axis];
 161
 162         // -----Get the desired angle rate depending on flight mode
 163         int32_t angleRate;
 164         if (axis == FD_YAW) {
 165             // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
 166             angleRate = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
 167         } else {
 168             // control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
 169             angleRate = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
 170             if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 171                 // calculate error angle and limit the angle to the max inclination
 172                 // multiplication of rcCommand corresponds to changing the sticks scaling here
 173 #ifdef GPS
 174                 const int32_t errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
 175                         - attitude.raw[axis] + angleTrim->raw[axis];
 176 #else
 177                 const int32_t errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
 178                         - attitude.raw[axis] + angleTrim->raw[axis];
 179 #endif
 180                 if (FLIGHT_MODE(ANGLE_MODE)) {
 181                     // ANGLE mode
 182                     angleRate = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
 183                 } else {
 184                     // HORIZON mode
 185                     // mix in errorAngle to desired angleRate to add a little auto-level feel.
 186                     // horizonLevelStrength has been scaled to the stick input
 187                     angleRate += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
 188                 }
 189             }
 190         }
 191
 192         // --------low-level gyro-based PID. ----------
 193         const int32_t gyroRate = gyroADC[axis] / 4;
 194         axisPID[axis] = pidMultiWiiRewriteCore(axis, pidProfile, gyroRate, angleRate);
 195
 196 #ifdef GTUNE
 197         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 198              calculate_Gtune(axis);
 199         }
 200 #endif
 201     }
 202 }
 203
 204 #endif