src/main/config/simplified_tuning.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <math.h>
  22 #include "platform.h"
  23
  24 #ifdef USE_SIMPLIFIED_TUNING
  25
  26 #include "common/axis.h"
  27 #include "common/maths.h"
  28
  29 #include "config/simplified_tuning.h"
  30
  31 #include "sensors/gyro.h"
  32
  33 static void calculateNewPidValues(pidProfile_t *pidProfile)
  34 {
  35     const pidf_t pidDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = {
  36             [PID_ROLL] = PID_ROLL_DEFAULT,
  37             [PID_PITCH] = PID_PITCH_DEFAULT,
  38             [PID_YAW] = PID_YAW_DEFAULT,
  39         };
  40
  41 #ifdef USE_D_MIN
  42     const int dMinDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = D_MIN_DEFAULT;
  43 #endif
  44     const float masterMultiplier = pidProfile->simplified_master_multiplier / 100.0f;
  45     const float piGain = pidProfile->simplified_pi_gain / 100.0f;
  46     const float dGain = pidProfile->simplified_d_gain / 100.0f;
  47     const float feedforwardGain = pidProfile->simplified_feedforward_gain / 100.0f;
  48     const float iGain = pidProfile->simplified_i_gain / 100.0f;
  49
  50     for (int axis = FD_ROLL; axis <= pidProfile->simplified_pids_mode; ++axis) {
  51         const float pitchDGain = (axis == FD_PITCH) ? pidProfile->simplified_roll_pitch_ratio / 100.0f : 1.0f;
  52         const float pitchPiGain = (axis == FD_PITCH) ? pidProfile->simplified_pitch_pi_gain / 100.0f : 1.0f;
  53         pidProfile->pid[axis].P = constrain(pidDefaults[axis].P * masterMultiplier * piGain * pitchPiGain, 0, PID_GAIN_MAX);
  54         pidProfile->pid[axis].I = constrain(pidDefaults[axis].I * masterMultiplier * piGain * iGain * pitchPiGain, 0, PID_GAIN_MAX);
  55 #ifdef USE_D_MIN
  56         const float dminRatio = (dMinDefaults[axis] > 0) ? 1.0f + (((float)pidDefaults[axis].D - dMinDefaults[axis]) / dMinDefaults[axis]) * (pidProfile->simplified_dmin_ratio / 100.0f) : 1.0f;
  57         pidProfile->pid[axis].D = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain * dminRatio, 0, PID_GAIN_MAX);
  58         pidProfile->d_min[axis] = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain, 0, PID_GAIN_MAX);
  59 #else
  60         pidProfile->pid[axis].D = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain, 0, PID_GAIN_MAX);
  61 #endif
  62         pidProfile->pid[axis].F = constrain(pidDefaults[axis].F * masterMultiplier * pitchPiGain * feedforwardGain, 0, F_GAIN_MAX);
  63     }
  64 }
  65
  66 static void calculateNewDTermFilterValues(pidProfile_t *pidProfile)
  67 {
  68     if (pidProfile->dterm_lpf1_dyn_min_hz) {
  69         pidProfile->dterm_lpf1_dyn_min_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  70         pidProfile->dterm_lpf1_dyn_max_hz = constrain(DTERM_LPF1_DYN_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  71     }
  72
  73     if (pidProfile->dterm_lpf1_static_hz) {
  74         pidProfile->dterm_lpf1_static_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  75     }
  76
  77     if (pidProfile->dterm_lpf2_static_hz) {
  78         pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ);
  79     }
  80
  81     if (!pidProfile->dterm_lpf1_type) {
  82         pidProfile->dterm_lpf1_type = FILTER_PT1;
  83     }
  84
  85     if (!pidProfile->dterm_lpf2_type) {
  86         pidProfile->dterm_lpf2_type = FILTER_PT1;
  87     }
  88 }
  89
  90 static void calculateNewGyroFilterValues()
  91 {
  92     if (gyroConfigMutable()->gyro_lpf1_dyn_min_hz) {
  93         gyroConfigMutable()->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  94         gyroConfigMutable()->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  95     }
  96
  97     if (gyroConfigMutable()->gyro_lpf1_static_hz) {
  98         gyroConfigMutable()->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  99     }
 100
 101     if (gyroConfigMutable()->gyro_lpf2_static_hz) {
 102         gyroConfigMutable()->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
 103     }
 104
 105     if (!gyroConfigMutable()->gyro_lpf1_type) {
 106         gyroConfigMutable()->gyro_lpf1_type = FILTER_PT1;
 107     }
 108
 109     if (!gyroConfigMutable()->gyro_lpf2_type) {
 110         gyroConfigMutable()->gyro_lpf2_type = FILTER_PT1;
 111     }
 112 }
 113
 114 void applySimplifiedTuning(pidProfile_t *pidProfile)
 115 {
 116     if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
 117         calculateNewPidValues(pidProfile);
 118     }
 119
 120     if (pidProfile->simplified_dterm_filter) {
 121         calculateNewDTermFilterValues(pidProfile);
 122     }
 123
 124     if (gyroConfig()->simplified_gyro_filter) {
 125         calculateNewGyroFilterValues();
 126     }
 127 }
 128
 129 void disableSimplifiedTuning(pidProfile_t *pidProfile)
 130 {
 131     pidProfile->simplified_pids_mode = PID_SIMPLIFIED_TUNING_OFF;
 132
 133     pidProfile->simplified_dterm_filter = false;
 134
 135     gyroConfigMutable()->simplified_gyro_filter = false;
 136 }
 137 #endif // USE_SIMPLIFIED_TUNING