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     pidProfile->dyn_lpf_dterm_min_hz = constrain(DYN_LPF_DTERM_MIN_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
  69     pidProfile->dyn_lpf_dterm_max_hz = constrain(DYN_LPF_DTERM_MAX_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
  70     pidProfile->dterm_lowpass2_hz = constrain(DTERM_LOWPASS_2_HZ_DEFAULT * pidProfile->simplified_dterm_filter_multiplier / 100, 0, FILTER_FREQUENCY_MAX);
  71     pidProfile->dterm_filter_type = FILTER_PT1;
  72     pidProfile->dterm_filter2_type = FILTER_PT1;
  73 }
  74
  75 static void calculateNewGyroFilterValues()
  76 {
  77     gyroConfigMutable()->dyn_lpf_gyro_min_hz = constrain(DYN_LPF_GYRO_MIN_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
  78     gyroConfigMutable()->dyn_lpf_gyro_max_hz = constrain(DYN_LPF_GYRO_MAX_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, DYN_LPF_FILTER_FREQUENCY_MAX);
  79     gyroConfigMutable()->gyro_lowpass2_hz = constrain(GYRO_LOWPASS_2_HZ_DEFAULT * gyroConfig()->simplified_gyro_filter_multiplier / 100, 0, FILTER_FREQUENCY_MAX);
  80     gyroConfigMutable()->gyro_lowpass_type = FILTER_PT1;
  81     gyroConfigMutable()->gyro_lowpass2_type = FILTER_PT1;
  82 }
  83
  84 void applySimplifiedTuning(pidProfile_t *pidProfile)
  85 {
  86     if (pidProfile->simplified_pids_mode != PID_SIMPLIFIED_TUNING_OFF) {
  87         calculateNewPidValues(pidProfile);
  88     }
  89
  90     if (pidProfile->simplified_dterm_filter) {
  91         calculateNewDTermFilterValues(pidProfile);
  92     }
  93
  94     if (gyroConfig()->simplified_gyro_filter) {
  95         calculateNewGyroFilterValues();
  96     }
  97 }
  98 #endif // USE_SIMPLIFIED_TUNING