src/main/flight/pid_autotune.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21 #include <math.h>
  22
  23 #include <platform.h>
  24
  25 #include "blackbox/blackbox.h"
  26 #include "blackbox/blackbox_fielddefs.h"
  27
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "drivers/time.h"
  35
  36 #include "config/feature.h"
  37 #include "config/parameter_group.h"
  38 #include "config/parameter_group_ids.h"
  39
  40 #include "fc/config.h"
  41 #include "fc/controlrate_profile.h"
  42 #include "fc/rc_controls.h"
  43 #include "fc/rc_modes.h"
  44 #include "fc/rc_adjustments.h"
  45 #include "fc/runtime_config.h"
  46 #include "fc/settings.h"
  47
  48 #include "flight/pid.h"
  49
  50 #define AUTOTUNE_FIXED_WING_MIN_FF              10
  51 #define AUTOTUNE_FIXED_WING_MAX_FF              255
  52 #define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40
  53 #define AUTOTUNE_FIXED_WING_MIN_YAW_RATE        10
  54 #define AUTOTUNE_FIXED_WING_MAX_RATE            720
  55 #define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE    10
  56 #define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL     20      // ms
  57 #define AUTOTUNE_FIXED_WING_SAMPLES             1000    // Use average over the last 20 seconds of hard maneuvers
  58 #define AUTOTUNE_FIXED_WING_MIN_SAMPLES         250     // Start updating tune after 5 seconds of hard maneuvers
  59
  60 PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2);
  61
  62 PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
  63     .fw_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_DEFAULT,
  64     .fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT,
  65     .fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT,
  66 );
  67
  68 typedef enum {
  69     DEMAND_TOO_LOW,
  70     DEMAND_UNDERSHOOT,
  71     DEMAND_OVERSHOOT,
  72     TUNE_UPDATED,
  73 } pidAutotuneState_e;
  74
  75 typedef struct {
  76     float   gainFF;
  77     float   rate;
  78     float   initialRate;
  79     float   absDesiredRateAccum;
  80     float   absReachedRateAccum;
  81     float   absPidOutputAccum;
  82     uint32_t updateCount;
  83 } pidAutotuneData_t;
  84
  85 #define AUTOTUNE_SAVE_PERIOD        5000        // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago
  86
  87 #if defined(USE_AUTOTUNE_FIXED_WING) || defined(USE_AUTOTUNE_MULTIROTOR)
  88
  89 static pidAutotuneData_t    tuneCurrent[XYZ_AXIS_COUNT];
  90 static pidAutotuneData_t    tuneSaved[XYZ_AXIS_COUNT];
  91 static timeMs_t             lastGainsUpdateTime;
  92
  93 void autotuneUpdateGains(pidAutotuneData_t * data)
  94 {
  95     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  96         pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
  97         ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f);
  98     }
  99     schedulePidGainsUpdate();
 100 }
 101
 102 void autotuneCheckUpdateGains(void)
 103 {
 104     const timeMs_t currentTimeMs = millis();
 105
 106     if ((currentTimeMs - lastGainsUpdateTime) < AUTOTUNE_SAVE_PERIOD) {
 107         return;
 108     }
 109
 110     // If pilot will exit autotune we'll restore values we are flying now
 111     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 112     autotuneUpdateGains(tuneSaved);
 113     lastGainsUpdateTime = currentTimeMs;
 114 }
 115
 116 void autotuneStart(void)
 117 {
 118     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 119         tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
 120         tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
 121         tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
 122         tuneCurrent[axis].absDesiredRateAccum = 0;
 123         tuneCurrent[axis].absReachedRateAccum = 0;
 124         tuneCurrent[axis].absPidOutputAccum = 0;
 125         tuneCurrent[axis].updateCount = 0;
 126     }
 127
 128     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 129     lastGainsUpdateTime = millis();
 130 }
 131
 132 void autotuneUpdateState(void)
 133 {
 134     if (isFwAutoModeActive(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) {
 135         if (!FLIGHT_MODE(AUTO_TUNE)) {
 136             autotuneStart();
 137             ENABLE_FLIGHT_MODE(AUTO_TUNE);
 138         }
 139         else {
 140             autotuneCheckUpdateGains();
 141         }
 142     } else {
 143         if (FLIGHT_MODE(AUTO_TUNE)) {
 144             autotuneUpdateGains(tuneSaved);
 145         }
 146
 147         DISABLE_FLIGHT_MODE(AUTO_TUNE);
 148     }
 149 }
 150
 151 static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
 152 {
 153 #ifndef USE_BLACKBOX
 154     UNUSED(adjustmentFunction);
 155     UNUSED(newValue);
 156 #else
 157     if (feature(FEATURE_BLACKBOX)) {
 158         flightLogEvent_inflightAdjustment_t eventData;
 159         eventData.adjustmentFunction = adjustmentFunction;
 160         eventData.newValue = newValue;
 161         eventData.floatFlag = false;
 162         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 163     }
 164 #endif
 165 }
 166
 167 #if defined(USE_AUTOTUNE_FIXED_WING)
 168
 169 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRate, float reachedRate, float pidOutput)
 170 {
 171     float maxRateSetting = tuneCurrent[axis].rate;
 172     float gainFF = tuneCurrent[axis].gainFF;
 173     float maxDesiredRate = maxRateSetting;
 174
 175     const float pidSumLimit = getPidSumLimit(axis);
 176     const float absDesiredRate = fabsf(desiredRate);
 177     const float absReachedRate = fabsf(reachedRate);
 178     const float absPidOutput = fabsf(pidOutput);
 179     const bool correctDirection = (desiredRate>0) == (reachedRate>0);
 180     float rateFullStick;
 181
 182     bool gainsUpdated = false;
 183     bool ratesUpdated = false;
 184
 185     const timeMs_t currentTimeMs = millis();
 186     static timeMs_t previousSampleTimeMs = 0;
 187     const timeDelta_t timeSincePreviousSample = currentTimeMs - previousSampleTimeMs;
 188
 189     // Use different max rate in ANLGE mode
 190     if (FLIGHT_MODE(ANGLE_MODE)) {
 191         float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P * FP_PID_LEVEL_P_MULTIPLIER;
 192         maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
 193     }
 194
 195     const float stickInput = absDesiredRate / maxDesiredRate;
 196
 197     if ((stickInput > (pidAutotuneConfig()->fw_min_stick / 100.0f)) && correctDirection && (timeSincePreviousSample >= AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL)) {
 198         // Record values every 20ms and calculate moving average over samples
 199         tuneCurrent[axis].updateCount++;
 200         tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 201         tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 202         tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 203
 204         if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
 205             if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
 206
 207                 // Target 80% control surface deflection to leave some room for P and I to work
 208                 float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
 209
 210                 // Theoretically achievable rate with target deflection
 211                 rateFullStick = pidSumTarget / tuneCurrent[axis].absPidOutputAccum * tuneCurrent[axis].absReachedRateAccum;
 212
 213                 // Rate update
 214                 if (rateFullStick > (maxRateSetting + 10.0f)) {
 215                     maxRateSetting += 10.0f;
 216                 } else if (rateFullStick < (maxRateSetting - 10.0f)) {
 217                     maxRateSetting -= 10.0f;
 218                 }
 219
 220                 // Constrain to safe values
 221                 uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE;
 222                 uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate);
 223                 tuneCurrent[axis].rate = constrainf(maxRateSetting, minRate, maxRate);
 224                 ratesUpdated = true;
 225             }
 226
 227             // Update FF towards value needed to achieve current rate target
 228             gainFF += (tuneCurrent[axis].absPidOutputAccum / tuneCurrent[axis].absReachedRateAccum * FP_PID_RATE_FF_MULTIPLIER - gainFF) * (AUTOTUNE_FIXED_WING_CONVERGENCE_RATE / 100.0f);
 229             tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF);
 230             gainsUpdated = true;
 231         }
 232
 233         // Reset timer
 234         previousSampleTimeMs = currentTimeMs;
 235     }
 236
 237     if (gainsUpdated) {
 238         autotuneUpdateGains(tuneCurrent);
 239
 240         switch (axis) {
 241             case FD_ROLL:
 242                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
 243                 break;
 244
 245             case FD_PITCH:
 246                 blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
 247                 break;
 248
 249             case FD_YAW:
 250                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
 251                 break;
 252         }
 253         gainsUpdated = false;
 254     }
 255
 256     if (ratesUpdated) {
 257         autotuneUpdateGains(tuneCurrent);
 258
 259         switch (axis) {
 260             case FD_ROLL:
 261                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate);
 262                 break;
 263
 264             case FD_PITCH:
 265                 blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate);
 266                 break;
 267
 268             case FD_YAW:
 269                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate);
 270                 break;
 271         }
 272
 273         // Debug
 274         DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick);
 275         ratesUpdated = false;
 276     }
 277 }
 278 #endif
 279
 280 #endif