src/main/flight/ez_tune.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include "fc/config.h"
  26 #include "config/config_reset.h"
  27 #include "config/parameter_group.h"
  28 #include "config/parameter_group_ids.h"
  29
  30 #include "flight/ez_tune.h"
  31
  32 #include "fc/settings.h"
  33 #include "flight/pid.h"
  34 #include "sensors/gyro.h"
  35 #include "fc/controlrate_profile.h"
  36
  37 #include "rx/rx.h"
  38
  39 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 1);
  40
  41 PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
  42     .enabled = SETTING_EZ_ENABLED_DEFAULT,
  43     .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT,
  44     .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT,
  45     .response = SETTING_EZ_RESPONSE_DEFAULT,
  46     .damping = SETTING_EZ_DAMPING_DEFAULT,
  47     .stability = SETTING_EZ_STABILITY_DEFAULT,
  48     .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT,
  49     .rate = SETTING_EZ_RATE_DEFAULT,
  50     .expo = SETTING_EZ_EXPO_DEFAULT,
  51     .snappiness = SETTING_EZ_SNAPPINESS_DEFAULT,
  52 );
  53
  54 #define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 }
  55 #define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 }
  56
  57 #define EZ_TUNE_YAW_SCALE 0.5f
  58
  59 static float computePt1FilterDelayMs(uint8_t filterHz) {
  60     return 1000.0f / (2.0f * M_PIf * filterHz);
  61 }
  62
  63 static float getYawPidScale(float input) {
  64     const float normalized = (input - 100) * 0.01f;
  65
  66     return 1.0f + (normalized * 0.5f);
  67 }
  68
  69 /**
  70  * Update INAV settings based on current EZTune settings
  71  * This has to be called every time control profile is changed, or EZTune settings are changed
  72  */
  73 void ezTuneUpdate(void) {
  74     if (ezTune()->enabled) {
  75
  76         //Enforce RC auto smoothing
  77         rxConfigMutable()->autoSmooth = 1;
  78
  79         // Setup filtering
  80         //Set Dterm LPF
  81         pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50);
  82         pidProfileMutable()->dterm_lpf_type = FILTER_PT2;
  83
  84         //Set main gyro filter
  85         gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
  86
  87         //Set anti-aliasing filter
  88         gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = SETTING_GYRO_ANTI_ALIASING_LPF_HZ_DEFAULT;
  89
  90         //Enable Smith predictor
  91         pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz);
  92
  93 #ifdef USE_DYNAMIC_FILTERS
  94         //Enable dynamic notch
  95         gyroConfigMutable()->dynamicGyroNotchEnabled = 1;
  96         gyroConfigMutable()->dynamicGyroNotchQ = 250;
  97         gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT);
  98         gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D;
  99 #endif
 100
 101 #ifdef USE_GYRO_KALMAN
 102         //Make sure Kalman filter is enabled
 103         gyroConfigMutable()->kalmanEnabled = 1;
 104         if (ezTune()->filterHz < 150) {
 105             gyroConfigMutable()->kalman_q = 200;
 106         } else {
 107             gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
 108         }
 109 #endif
 110
 111         //Disable dynamic LPF
 112         gyroConfigMutable()->gyroFilterMode = GYRO_FILTER_MODE_STATIC;
 113
 114         //Setup PID controller
 115
 116         const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT;
 117         const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT;
 118         const float pitchRatio = ezTune()->axisRatio / 100.0f;
 119
 120         //Roll
 121         pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f;
 122         pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f;
 123         pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f;
 124         pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f;
 125
 126         //Pitch
 127         pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio;
 128         pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio;
 129         pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio;
 130         pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio;
 131
 132         //Yaw
 133         pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response);
 134         pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability);
 135         pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping);
 136         pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness);
 137
 138         //Setup rates
 139         ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_ROLL] = scaleRange(ezTune()->rate, 0, 200, 30, 90);
 140         ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_PITCH] = scaleRange(ezTune()->rate, 0, 200, 30, 90);
 141         ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rates[FD_YAW] = scaleRange(ezTune()->rate, 0, 200, 30, 90) - 10;
 142
 143         ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
 144         ((controlRateConfig_t*)currentControlRateProfile)->stabilized.rcYawExpo8 = scaleRange(ezTune()->rate, 0, 200, 40, 100);
 145
 146         //D-Boost snappiness
 147         pidProfileMutable()->dBoostMin = scaleRangef(ezTune()->snappiness, 0, 100, 1.0f, 0.0f);
 148
 149     }
 150 }