src/main/flight/pid_luxfloat.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_LUXFLOAT_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 #ifndef SKIP_PID_LUXFLOAT
  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/runtime_config.h"
  48 #include "fc/rc_controls.h"
  49 #include "fc/rate_profile.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 float dT;
  59 extern uint8_t PIDweight[3];
  60 extern float lastITermf[3], ITermLimitf[3];
  61
  62 extern pt1Filter_t deltaFilter[3];
  63 extern pt1Filter_t yawFilter;
  64
  65 extern bool dtermNotchInitialised, dtermBiquadLpfInitialised;
  66 extern biquadFilter_t dtermFilterNotch[3];
  67 extern biquadFilter_t dtermFilterLpf[3];
  68
  69 extern uint8_t motorCount;
  70
  71 #ifdef BLACKBOX
  72 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
  73 #endif
  74
  75 // constants to scale pidLuxFloat so output is same as pidMultiWiiRewrite
  76 static const float luxPTermScale = 1.0f / 128;
  77 static const float luxITermScale = 1000000.0f / 0x1000000;
  78 static const float luxDTermScale = (0.000001f * (float)0xFFFF) / 512;
  79 static const float luxGyroScale = 16.4f / 4; // the 16.4 is needed because mwrewrite does not scale according to the gyro model gyro.scale
  80
  81 STATIC_UNIT_TESTED int16_t pidLuxFloatCore(int axis, const pidProfile_t *pidProfile, float gyroRate, float angleRate)
  82 {
  83     static float lastRateForDelta[3];
  84
  85     SET_PID_LUX_FLOAT_CORE_LOCALS(axis);
  86
  87     pidInitFilters(pidProfile);
  88
  89     const float rateError = angleRate - gyroRate;
  90
  91     // -----calculate P component
  92     float PTerm = luxPTermScale * rateError * pidProfile->P8[axis] * PIDweight[axis] / 100;
  93     // Constrain YAW by yaw_p_limit value if not servo driven, in that case servolimits apply
  94     if (axis == YAW) {
  95         if (pidProfile->yaw_lpf_hz) {
  96             PTerm = pt1FilterApply4(&yawFilter, PTerm, pidProfile->yaw_lpf_hz, getdT());
  97         }
  98         if (pidProfile->yaw_p_limit && motorCount >= 4) {
  99             PTerm = constrainf(PTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
 100         }
 101     }
 102
 103     // -----calculate I component
 104     float ITerm = lastITermf[axis] + luxITermScale * rateError * getdT() * pidProfile->I8[axis];
 105     // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
 106     // I coefficient (I8) moved before integration to make limiting independent from PID settings
 107     ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
 108     // Anti windup protection
 109     if (rcModeIsActive(BOXAIRMODE)) {
 110         if (STATE(ANTI_WINDUP) || motorLimitReached) {
 111             ITerm = constrainf(ITerm, -ITermLimitf[axis], ITermLimitf[axis]);
 112         } else {
 113             ITermLimitf[axis] = ABS(ITerm);
 114         }
 115     }
 116     lastITermf[axis] = ITerm;
 117
 118     // -----calculate D component
 119     float DTerm;
 120     if (pidProfile->D8[axis] == 0) {
 121         // optimisation for when D8 is zero, often used by YAW axis
 122         DTerm = 0;
 123     } else {
 124         float delta;
 125         if (pidProfile->deltaMethod == PID_DELTA_FROM_MEASUREMENT) {
 126             delta = -(gyroRate - lastRateForDelta[axis]);
 127             lastRateForDelta[axis] = gyroRate;
 128         } else {
 129             delta = rateError - lastRateForDelta[axis];
 130             lastRateForDelta[axis] = rateError;
 131         }
 132         // Divide delta by targetLooptime to get differential (ie dr/dt)
 133         delta *= (1.0f / getdT());
 134
 135         // Filter delta
 136         if (dtermNotchInitialised) delta = biquadFilterApply(&dtermFilterNotch[axis], delta);
 137
 138         if (pidProfile->dterm_lpf_hz) {
 139             if (dtermBiquadLpfInitialised) {
 140                 delta = biquadFilterApply(&dtermFilterLpf[axis], delta);
 141             } else {
 142                 // DTerm delta low pass filter
 143                 delta = pt1FilterApply4(&deltaFilter[axis], delta, pidProfile->dterm_lpf_hz, getdT());
 144             }
 145         }
 146         DTerm = luxDTermScale * delta * pidProfile->D8[axis] * PIDweight[axis] / 100;
 147         DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
 148     }
 149
 150 #ifdef BLACKBOX
 151     axisPID_P[axis] = PTerm;
 152     axisPID_I[axis] = ITerm;
 153     axisPID_D[axis] = DTerm;
 154 #endif
 155     GET_PID_LUX_FLOAT_CORE_LOCALS(axis);
 156     // -----calculate total PID output
 157     return lrintf(PTerm + ITerm + DTerm);
 158 }
 159
 160 void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
 161         uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig)
 162 {
 163     float horizonLevelStrength = 0.0f;
 164     if (FLIGHT_MODE(HORIZON_MODE)) {
 165         // (convert 0-100 range to 0.0-1.0 range)
 166         horizonLevelStrength = (float)calcHorizonLevelStrength(rxConfig->midrc, pidProfile->horizon_tilt_effect,
 167                 pidProfile->horizon_tilt_mode, pidProfile->D8[PIDLEVEL]) / 100.0f;
 168     }
 169
 170     // ----------PID controller----------
 171     for (int axis = 0; axis < 3; axis++) {
 172         const uint8_t rate = controlRateConfig->rates[axis];
 173
 174         // -----Get the desired angle rate depending on flight mode
 175         float angleRate;
 176         if (axis == FD_YAW) {
 177             // YAW is always gyro-controlled (MAG correction is applied to rcCommand) 100dps to 1100dps max yaw rate
 178             angleRate = (float)((rate + 27) * rcCommand[YAW]) / 32.0f;
 179         } else {
 180             // control is GYRO based for ACRO and HORIZON - direct sticks control is applied to rate PID
 181             angleRate = (float)((rate + 27) * rcCommand[axis]) / 16.0f; // 200dps to 1200dps max roll/pitch rate
 182             if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
 183                 // calculate error angle and limit the angle to the max inclination
 184                 // multiplication of rcCommand corresponds to changing the sticks scaling here
 185 #ifdef GPS
 186                 const float errorAngle = constrain(2 * rcCommand[axis] + GPS_angle[axis], -((int)max_angle_inclination), max_angle_inclination)
 187                         - attitude.raw[axis] + angleTrim->raw[axis];
 188 #else
 189                 const float errorAngle = constrain(2 * rcCommand[axis], -((int)max_angle_inclination), max_angle_inclination)
 190                         - attitude.raw[axis] + angleTrim->raw[axis];
 191 #endif
 192                 if (FLIGHT_MODE(ANGLE_MODE)) {
 193                     // ANGLE mode
 194                     angleRate = errorAngle * pidProfile->P8[PIDLEVEL] / 16.0f;
 195                 } else {
 196                     // HORIZON mode
 197                     // mix in errorAngle to desired angleRate to add a little auto-level feel.
 198                     // horizonLevelStrength has been scaled to the stick input
 199                     angleRate += errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 16.0f;
 200                 }
 201             }
 202         }
 203
 204         // --------low-level gyro-based PID. ----------
 205         const float gyroRate = luxGyroScale * gyroADC[axis] * gyro.scale;
 206         axisPID[axis] = pidLuxFloatCore(axis, pidProfile, gyroRate, angleRate);
 207         //axisPID[axis] = constrain(axisPID[axis], -PID_LUX_FLOAT_MAX_PID, PID_LUX_FLOAT_MAX_PID);
 208 #ifdef GTUNE
 209         if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
 210             calculate_Gtune(axis);
 211         }
 212 #endif
 213     }
 214 }
 215
 216 #endif
 217