src/main/flight/pid_mw23.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 <string.h>
  21 #include <math.h>
  22
  23 #include <platform.h>
  24
  25 #include "build/build_config.h"
  26
  27 #ifdef USE_PID_MW23
  28
  29 #include "common/axis.h"
  30 #include "common/maths.h"
  31 #include "common/filter.h"
  32
  33 #include "config/parameter_group.h"
  34 #include "config/config_unittest.h"
  35
  36 #include "drivers/sensor.h"
  37 #include "drivers/accgyro.h"
  38 #include "drivers/gyro_sync.h"
  39
  40 #include "sensors/sensors.h"
  41 #include "sensors/gyro.h"
  42 #include "sensors/acceleration.h"
  43
  44 #include "rx/rx.h"
  45
  46 #include "fc/rc_controls.h"
  47 #include "fc/rate_profile.h"
  48 #include "fc/runtime_config.h"
  49
  50 #include "flight/pid.h"
  51 #include "flight/imu.h"
  52 #include "flight/navigation.h"
  53 #include "flight/gtune.h"
  54 #include "flight/mixer.h"
  55
  56 static int32_t ITermAngle[2];
  57
  58 uint8_t dynP8[3], dynI8[3], dynD8[3];
  59
  60 extern float dT;
  61 extern uint8_t motorCount;
  62
  63 #ifdef BLACKBOX
  64 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  65 #endif
  66 extern int32_t lastITerm[3], ITermLimit[3];
  67
  68 extern pt1Filter_t deltaFilter[3];
  69
  70
  71 void pidResetITermAngle(void)
  72 {
  73     ITermAngle[AI_ROLL] = 0;
  74     ITermAngle[AI_PITCH] = 0;
  75 }
  76
  77 void pidMultiWii23(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
  78         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
  79 {
  80     UNUSED(rxConfig);
  81
  82     pidInitFilters(pidProfile);
  83
  84     int axis, prop = 0;
  85     int32_t rc, error, errorAngle, delta, gyroError;
  86     int32_t PTerm, ITerm, PTermACC, ITermACC, DTerm;
  87     static int16_t lastErrorForDelta[2];
  88     static int32_t delta1[2], delta2[2];
  89
  90     if (FLIGHT_MODE(HORIZON_MODE)) {
  91         prop = MIN(MAX(ABS(rcCommand[PITCH]), ABS(rcCommand[ROLL])), 512);
  92     }
  93
  94     // PITCH & ROLL
  95     for (axis = 0; axis < 2; axis++) {
  96
  97         rc = rcCommand[axis] << 1;
  98
  99         gyroError = gyroADC[axis] / 4;
 100
 101         error = rc - gyroError;
 102         lastITerm[axis]  = constrain(lastITerm[axis] + error, -16000, +16000);   // WindUp   16 bits is ok here
 103
 104         if (ABS(gyroADC[axis]) > (640 * 4)) {
 105             lastITerm[axis] = 0;
 106         }
 107
 108         // Anti windup protection
 109         if (rcModeIsActive(BOXAIRMODE)) {
 110             if (STATE(ANTI_WINDUP) || motorLimitReached) {
 111                 lastITerm[axis] = constrain(lastITerm[axis], -ITermLimit[axis], ITermLimit[axis]);
 112             } else {
 113                 ITermLimit[axis] = ABS(lastITerm[axis]);
 114             }
 115         }
 116
 117         ITerm = (lastITerm[axis] >> 7) * pidProfile->I8[axis] >> 6;   // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
 118
 119         PTerm = (int32_t)rc * pidProfile->P8[axis] >> 6;
 120
 121         if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {   // axis relying on ACC
 122             // 50 degrees max inclination
 123 #ifdef GPS
 124             errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination),
 125                 +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 126 #else
 127             errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
 128                 +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis];
 129 #endif
 130
 131             ITermAngle[axis]  = constrain(ITermAngle[axis] + errorAngle, -10000, +10000);                                                // WindUp     //16 bits is ok here
 132
 133             PTermACC = ((int32_t)errorAngle * pidProfile->P8[PIDLEVEL]) >> 7;   // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768   16 bits is ok for result
 134
 135             int16_t limit = pidProfile->D8[PIDLEVEL] * 5;
 136             PTermACC = constrain(PTermACC, -limit, +limit);
 137
 138             ITermACC = ((int32_t)ITermAngle[axis] * pidProfile->I8[PIDLEVEL]) >> 12;  // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
 139
 140             ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
 141             PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
 142         }
 143
 144         PTerm -= ((int32_t)gyroError * dynP8[axis]) >> 6;   // 32 bits is needed for calculation
 145
 146         //-----calculate D-term based on the configured approach (delta from measurement or deltafromError)
 147         // Delta from measurement
 148         delta = -(gyroError - lastErrorForDelta[axis]);
 149         lastErrorForDelta[axis] = gyroError;
 150         if (pidProfile->dterm_lpf_hz) {
 151             // Dterm delta low pass
 152             DTerm = delta;
 153             DTerm = lrintf(pt1FilterApply4(&deltaFilter[axis], (float)DTerm, pidProfile->dterm_lpf_hz, dT)) * 3;  // Keep same scaling as unfiltered DTerm
 154         } else {
 155             // When dterm filter disabled apply moving average to reduce noise
 156             DTerm  = delta1[axis] + delta2[axis] + delta;
 157             delta2[axis] = delta1[axis];
 158             delta1[axis] = delta;
 159         }
 160         DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;   // 32 bits is needed for calculation
 161
 162         axisPID[axis] = PTerm + ITerm + DTerm;
 163
 164 #ifdef GTUNE
 165         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 166             calculate_Gtune(axis);
 167         }
 168 #endif
 169
 170 #ifdef BLACKBOX
 171         axisPID_P[axis] = PTerm;
 172         axisPID_I[axis] = ITerm;
 173         axisPID_D[axis] = DTerm;
 174 #endif
 175     }
 176
 177     //YAW
 178     rc = (int32_t)rcCommand[YAW] * (2 * controlRateConfig->rates[YAW] + 30)  >> 5;
 179 #ifdef ALIENWFLIGHT
 180     error = rc - gyroADC[FD_YAW];
 181 #else
 182     error = rc - (gyroADC[FD_YAW] / 4);
 183 #endif
 184     lastITerm[FD_YAW]  += (int32_t)error * pidProfile->I8[FD_YAW];
 185     lastITerm[FD_YAW]  = constrain(lastITerm[FD_YAW], 2 - ((int32_t)1 << 28), -2 + ((int32_t)1 << 28));
 186     if (ABS(rc) > 50) lastITerm[FD_YAW] = 0;
 187
 188     PTerm = (int32_t)error * pidProfile->P8[FD_YAW] >> 6; // TODO: Bitwise shift on a signed integer is not recommended
 189
 190     // Constrain YAW by D value if not servo driven in that case servolimits apply
 191     if(motorCount >= 4 && pidProfile->yaw_p_limit < YAW_P_LIMIT_MAX) {
 192         PTerm = constrain(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 193     }
 194
 195     ITerm = constrain((int16_t)(lastITerm[FD_YAW] >> 13), -GYRO_I_MAX, +GYRO_I_MAX);
 196
 197     axisPID[FD_YAW] =  PTerm + ITerm;
 198
 199 #ifdef GTUNE
 200     if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 201         calculate_Gtune(FD_YAW);
 202     }
 203 #endif
 204
 205 #ifdef BLACKBOX
 206     axisPID_P[FD_YAW] = PTerm;
 207     axisPID_I[FD_YAW] = ITerm;
 208     axisPID_D[FD_YAW] = 0;
 209 #endif
 210 }
 211
 212 #endif