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
  36 PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(ezTuneSettings_t, ezTune, PG_EZ_TUNE, 0);
  37
  38 PG_RESET_TEMPLATE(ezTuneSettings_t, ezTune,
  39     .enabled = SETTING_EZ_ENABLED_DEFAULT,
  40     .filterHz = SETTING_EZ_FILTER_HZ_DEFAULT,
  41     .axisRatio = SETTING_EZ_AXIS_RATIO_DEFAULT,
  42     .response = SETTING_EZ_RESPONSE_DEFAULT,
  43     .damping = SETTING_EZ_DAMPING_DEFAULT,
  44     .stability = SETTING_EZ_STABILITY_DEFAULT,
  45     .aggressiveness = SETTING_EZ_AGGRESSIVENESS_DEFAULT,
  46     .rate = SETTING_EZ_RATE_DEFAULT,
  47     .expo = SETTING_EZ_EXPO_DEFAULT,
  48 );
  49
  50 #define EZ_TUNE_PID_RP_DEFAULT { 40, 75, 23, 100 }
  51 #define EZ_TUNE_PID_YAW_DEFAULT { 45, 80, 0, 100 }
  52
  53 #define EZ_TUNE_YAW_SCALE 0.5f
  54
  55 static float computePt1FilterDelayMs(uint8_t filterHz) {
  56     return 1.0f / (2.0f * M_PIf * filterHz);
  57 }
  58
  59 static float getYawPidScale(float input) {
  60     const float normalized = (input - 100) * 0.01f;
  61
  62     return 1.0f + (normalized * 0.5f);
  63 }
  64
  65 /**
  66  * Update INAV settings based on current EZTune settings
  67  * This has to be called every time control profile is changed, or EZTune settings are changed
  68  * FIXME call on EZTune settings change
  69  */
  70 void ezTuneUpdate(void) {
  71     if (ezTune()->enabled) {
  72
  73         // Setup filtering
  74
  75         //Enable Smith predictor
  76         pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz)
  77             + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz);
  78
  79         //Set Dterm LPF
  80         pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50);
  81         pidProfileMutable()->dterm_lpf_type = FILTER_PT2;
  82
  83         //Set main gyro filter
  84         gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
  85         gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
  86
  87         //Set anti-aliasing filter
  88         gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(MIN(ezTune()->filterHz * 2, 250), getGyroLooptime() * 0.5f);
  89         gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
  90
  91         //Enable dynamic notch
  92         gyroConfigMutable()->dynamicGyroNotchEnabled = 1;
  93         gyroConfigMutable()->dynamicGyroNotchQ = 250;
  94         gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT);
  95         gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D;
  96
  97         //Make sure Kalman filter is enabled
  98         gyroConfigMutable()->kalmanEnabled = 1;
  99         if (ezTune()->filterHz < 150) {
 100             gyroConfigMutable()->kalman_q = 200;
 101         } else {
 102             gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
 103         }
 104
 105         //Disable dynamic LPF
 106         gyroConfigMutable()->useDynamicLpf = 0;
 107
 108         //Setup PID controller
 109
 110         const uint8_t pidDefaults[4] = EZ_TUNE_PID_RP_DEFAULT;
 111         const uint8_t pidDefaultsYaw[4] = EZ_TUNE_PID_YAW_DEFAULT;
 112         const float pitchRatio = ezTune()->axisRatio / 100.0f;
 113
 114         //Roll
 115         pidProfileMutable()->bank_mc.pid[PID_ROLL].P = pidDefaults[0] * ezTune()->response / 100.0f;
 116         pidProfileMutable()->bank_mc.pid[PID_ROLL].I = pidDefaults[1] * ezTune()->stability / 100.0f;
 117         pidProfileMutable()->bank_mc.pid[PID_ROLL].D = pidDefaults[2] * ezTune()->damping / 100.0f;
 118         pidProfileMutable()->bank_mc.pid[PID_ROLL].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f;
 119
 120         //Pitch
 121         pidProfileMutable()->bank_mc.pid[PID_PITCH].P = pidDefaults[0] * ezTune()->response / 100.0f * pitchRatio;
 122         pidProfileMutable()->bank_mc.pid[PID_PITCH].I = pidDefaults[1] * ezTune()->stability / 100.0f * pitchRatio;
 123         pidProfileMutable()->bank_mc.pid[PID_PITCH].D = pidDefaults[2] * ezTune()->damping / 100.0f * pitchRatio;
 124         pidProfileMutable()->bank_mc.pid[PID_PITCH].FF = pidDefaults[3] * ezTune()->aggressiveness / 100.0f * pitchRatio;
 125
 126         //Yaw
 127         pidProfileMutable()->bank_mc.pid[PID_YAW].P = pidDefaultsYaw[0] * getYawPidScale(ezTune()->response);
 128         pidProfileMutable()->bank_mc.pid[PID_YAW].I = pidDefaultsYaw[1] * getYawPidScale(ezTune()->stability);
 129         pidProfileMutable()->bank_mc.pid[PID_YAW].D = pidDefaultsYaw[2] * getYawPidScale(ezTune()->damping);
 130         pidProfileMutable()->bank_mc.pid[PID_YAW].FF = pidDefaultsYaw[3] * getYawPidScale(ezTune()->aggressiveness);
 131     }
 132 }