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_TUNE_ENABLED_DEFAULT,
  40     .filterHz = SETTING_EZ_TUNE_FILTER_HZ_DEFAULT,
  41     .axisRatio = SETTING_EZ_TUNE_AXIS_RATIO_DEFAULT,
  42     .response = SETTING_EZ_TUNE_RESPONSE_DEFAULT,
  43     .damping = SETTING_EZ_TUNE_DAMPING_DEFAULT,
  44     .stability = SETTING_EZ_TUNE_STABILITY_DEFAULT,
  45     .aggressiveness = SETTING_EZ_TUNE_AGGRESSIVENESS_DEFAULT,
  46 );
  47
  48 static float computePt1FilterDelayMs(uint8_t filterHz) {
  49     return 1.0f / (2.0f * M_PIf * filterHz);
  50 }
  51
  52 /**
  53  * Update INAV settings based on current EZTune settings
  54  * This has to be called every time control profile is changed, or EZTune settings are changed
  55  * FIXME call on profile change
  56  * FIXME call on EZTune settings change
  57  */
  58 void ezTuneUpdate(void) {
  59     if (ezTune()->enabled) {
  60
  61         // Setup filtering
  62
  63         //Enable Smith predictor
  64         pidProfileMutable()->smithPredictorDelay = computePt1FilterDelayMs(ezTune()->filterHz)
  65             + computePt1FilterDelayMs(gyroConfig()->gyro_anti_aliasing_lpf_hz);
  66
  67         //Set Dterm LPF
  68         pidProfileMutable()->dterm_lpf_hz = MAX(ezTune()->filterHz - 5, 50);
  69         pidProfileMutable()->dterm_lpf_type = FILTER_PT2;
  70
  71         //Set main gyro filter
  72         gyroConfigMutable()->gyro_main_lpf_hz = ezTune()->filterHz;
  73         gyroConfigMutable()->gyro_main_lpf_type = FILTER_PT1;
  74
  75         //Set anti-aliasing filter
  76         gyroConfigMutable()->gyro_anti_aliasing_lpf_hz = MIN(ezTune()->filterHz * 2, 250);
  77         gyroConfigMutable()->gyro_anti_aliasing_lpf_type = FILTER_PT1;
  78
  79         //Enable dynamic notch
  80         gyroConfigMutable()->dynamicGyroNotchEnabled = 1;
  81         gyroConfigMutable()->dynamicGyroNotchQ = 250;
  82         gyroConfigMutable()->dynamicGyroNotchMinHz = MAX(ezTune()->filterHz * 0.667f, SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT);
  83         gyroConfigMutable()->dynamicGyroNotchMode = DYNAMIC_NOTCH_MODE_3D;
  84
  85         //Make sure Kalman filter is enabled
  86         gyroConfigMutable()->kalmanEnabled = 1;
  87         if (ezTune()->filterHz < 150) {
  88             gyroConfigMutable()->kalman_q = 200;
  89         } else {
  90             gyroConfigMutable()->kalman_q = scaleRangef(ezTune()->filterHz, 150, 300, 200, 400);
  91         }
  92
  93         //Disable dynamic LPF
  94         gyroConfigMutable()->useDynamicLpf = 0;
  95
  96         //Setup PID controller
  97
  98         //Roll
  99
 100
 101     }
 102 }