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 static void calculateNewPidValues(pidProfile_t *pidProfile)
  32 {
  33     const pidf_t pidDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = {
  34             [PID_ROLL] = PID_ROLL_DEFAULT,
  35             [PID_PITCH] = PID_PITCH_DEFAULT,
  36             [PID_YAW] = PID_YAW_DEFAULT,
  37         };
  38
  39 #ifdef USE_D_MIN
  40     const int dMinDefaults[FLIGHT_DYNAMICS_INDEX_COUNT] = D_MIN_DEFAULT;
  41 #endif
  42     const float masterMultiplier = pidProfile->simplified_master_multiplier / 100.0f;
  43     const float piGain = pidProfile->simplified_pi_gain / 100.0f;
  44     const float dGain = pidProfile->simplified_d_gain / 100.0f;
  45     const float feedforwardGain = pidProfile->simplified_feedforward_gain / 100.0f;
  46     const float iGain = pidProfile->simplified_i_gain / 100.0f;
  47
  48     for (int axis = FD_ROLL; axis <= pidProfile->simplified_pids_mode; ++axis) {
  49         const float pitchDGain = (axis == FD_PITCH) ? pidProfile->simplified_roll_pitch_ratio / 100.0f : 1.0f;
  50         const float pitchPiGain = (axis == FD_PITCH) ? pidProfile->simplified_pitch_pi_gain / 100.0f : 1.0f;
  51         pidProfile->pid[axis].P = constrain(pidDefaults[axis].P * masterMultiplier * piGain * pitchPiGain, 0, PID_GAIN_MAX);
  52         pidProfile->pid[axis].I = constrain(pidDefaults[axis].I * masterMultiplier * piGain * iGain * pitchPiGain, 0, PID_GAIN_MAX);
  53 #ifdef USE_D_MIN
  54         const float dminRatio = (dMinDefaults[axis] > 0) ? 1.0f + (((float)pidDefaults[axis].D - dMinDefaults[axis]) / dMinDefaults[axis]) * (pidProfile->simplified_dmin_ratio / 100.0f) : 1.0f;
  55         pidProfile->pid[axis].D = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain * dminRatio, 0, PID_GAIN_MAX);
  56         pidProfile->d_min[axis] = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain, 0, PID_GAIN_MAX);
  57 #else
  58         pidProfile->pid[axis].D = constrain(dMinDefaults[axis] * masterMultiplier * dGain * pitchDGain, 0, PID_GAIN_MAX);
  59 #endif
  60         pidProfile->pid[axis].F = constrain(pidDefaults[axis].F * masterMultiplier * pitchPiGain * feedforwardGain, 0, F_GAIN_MAX);
  61     }
  62 }
  63
  64 static void calculateNewDTermFilterValues(pidProfile_t *pidProfile)
  65 {
  66     if (pidProfile->dterm_lpf1_dyn_min_hz) {
  67         pidProfile->dterm_lpf1_dyn_min_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  68         pidProfile->dterm_lpf1_dyn_max_hz = constrain(DTERM_LPF1_DYN_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  69     }
  70
  71     if (pidProfile->dterm_lpf1_static_hz) {
  72         pidProfile->dterm_lpf1_static_hz = constrain(DTERM_LPF1_DYN_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  73     }
  74
  75     if (pidProfile->dterm_lpf2_static_hz) {
  76         pidProfile->dterm_lpf2_static_hz = constrain(DTERM_LPF2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, LPF_MAX_HZ);
  77     }
  78 }
  79
  80 static void calculateNewGyroFilterValues(gyroConfig_t *gyroConfig)
  81 {
  82     if (gyroConfig->gyro_lpf1_dyn_min_hz) {
  83         gyroConfig->gyro_lpf1_dyn_min_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  84         gyroConfig->gyro_lpf1_dyn_max_hz = constrain(GYRO_LPF1_DYN_MAX_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  85     }
  86
  87     if (gyroConfig->gyro_lpf1_static_hz) {
  88         gyroConfig->gyro_lpf1_static_hz = constrain(GYRO_LPF1_DYN_MIN_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_MAX_HZ);
  89     }
  90
  91     if (gyroConfig->gyro_lpf2_static_hz) {
  92         gyroConfig->gyro_lpf2_static_hz = constrain(GYRO_LPF2_HZ_DEFAULT * gyroConfig->simplified_gyro_filter_multiplier / 100, 0, LPF_MAX_HZ);
  93     }
  94 }
  95
  96 void applySimplifiedTuningPids(pidProfile_t *pidProfile)
  97 {
  98     if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
  99         calculateNewPidValues(pidProfile);
 100     }
 101 }
 102
 103 void applySimplifiedTuningDtermFilters(pidProfile_t *pidProfile)
 104 {
 105     if (pidProfile->simplified_dterm_filter) {
 106         calculateNewDTermFilterValues(pidProfile);
 107     }
 108 }
 109
 110 void applySimplifiedTuningGyroFilters(gyroConfig_t *gyroConfig)
 111 {
 112     if (gyroConfig->simplified_gyro_filter) {
 113         calculateNewGyroFilterValues(gyroConfig);
 114     }
 115 }
 116
 117
 118 void applySimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig)
 119 {
 120     applySimplifiedTuningPids(pidProfile);
 121     applySimplifiedTuningDtermFilters(pidProfile);
 122     applySimplifiedTuningGyroFilters(gyroConfig);
 123 }
 124
 125 void disableSimplifiedTuning(pidProfile_t *pidProfile, gyroConfig_t *gyroConfig)
 126 {
 127     pidProfile->simplified_pids_mode = PID_SIMPLIFIED_TUNING_OFF;
 128
 129     pidProfile->simplified_dterm_filter = false;
 130
 131     gyroConfig->simplified_gyro_filter = false;
 132 }
 133 #endif // USE_SIMPLIFIED_TUNING