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_adjustments.h"
  44 #include "fc/runtime_config.h"
  45 #include "fc/settings.h"
  46
  47 #include "flight/pid.h"
  48
  49 #define AUTOTUNE_FIXED_WING_MIN_FF              10
  50 #define AUTOTUNE_FIXED_WING_MAX_FF              255
  51 #define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40
  52 #define AUTOTUNE_FIXED_WING_MIN_YAW_RATE        10
  53 #define AUTOTUNE_FIXED_WING_MAX_RATE            720
  54 #define AUTOTUNE_FIXED_WING_CONVERGENCE_RATE    10
  55 #define AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL     20      // ms
  56 #define AUTOTUNE_FIXED_WING_SAMPLES             1000    // Use average over the last 20 seconds of hard maneuvers
  57 #define AUTOTUNE_FIXED_WING_MIN_SAMPLES         250     // Start updating tune after 5 seconds of hard maneuvers
  58
  59 PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2);
  60
  61 PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
  62     .fw_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_DEFAULT,
  63     .fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT,
  64     .fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT,
  65 );
  66
  67 typedef enum {
  68     DEMAND_TOO_LOW,
  69     DEMAND_UNDERSHOOT,
  70     DEMAND_OVERSHOOT,
  71     TUNE_UPDATED,
  72 } pidAutotuneState_e;
  73
  74 typedef struct {
  75     float   gainFF;
  76     float   rate;
  77     float   initialRate;
  78     float   absDesiredRateAccum;
  79     float   absReachedRateAccum;
  80     float   absPidOutputAccum;
  81     uint32_t updateCount;
  82 } pidAutotuneData_t;
  83
  84 #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
  85
  86 #if defined(USE_AUTOTUNE_FIXED_WING) || defined(USE_AUTOTUNE_MULTIROTOR)
  87
  88 static pidAutotuneData_t    tuneCurrent[XYZ_AXIS_COUNT];
  89 static pidAutotuneData_t    tuneSaved[XYZ_AXIS_COUNT];
  90 static timeMs_t             lastGainsUpdateTime;
  91
  92 void autotuneUpdateGains(pidAutotuneData_t * data)
  93 {
  94     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  95         pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
  96         ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f);
  97     }
  98     schedulePidGainsUpdate();
  99 }
 100
 101 void autotuneCheckUpdateGains(void)
 102 {
 103     const timeMs_t currentTimeMs = millis();
 104
 105     if ((currentTimeMs - lastGainsUpdateTime) < AUTOTUNE_SAVE_PERIOD) {
 106         return;
 107     }
 108
 109     // If pilot will exit autotune we'll restore values we are flying now
 110     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 111     autotuneUpdateGains(tuneSaved);
 112     lastGainsUpdateTime = currentTimeMs;
 113 }
 114
 115 void autotuneStart(void)
 116 {
 117     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 118         tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
 119         tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
 120         tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
 121         tuneCurrent[axis].absDesiredRateAccum = 0;
 122         tuneCurrent[axis].absReachedRateAccum = 0;
 123         tuneCurrent[axis].absPidOutputAccum = 0;
 124         tuneCurrent[axis].updateCount = 0;
 125     }
 126
 127     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 128     lastGainsUpdateTime = millis();
 129 }
 130
 131 void autotuneUpdateState(void)
 132 {
 133     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && ARMING_FLAG(ARMED)) {
 134         if (!FLIGHT_MODE(AUTO_TUNE)) {
 135             autotuneStart();
 136             ENABLE_FLIGHT_MODE(AUTO_TUNE);
 137         }
 138         else {
 139             autotuneCheckUpdateGains();
 140         }
 141     } else {
 142         if (FLIGHT_MODE(AUTO_TUNE)) {
 143             autotuneUpdateGains(tuneSaved);
 144         }
 145
 146         DISABLE_FLIGHT_MODE(AUTO_TUNE);
 147     }
 148 }
 149
 150 static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
 151 {
 152 #ifndef USE_BLACKBOX
 153     UNUSED(adjustmentFunction);
 154     UNUSED(newValue);
 155 #else
 156     if (feature(FEATURE_BLACKBOX)) {
 157         flightLogEvent_inflightAdjustment_t eventData;
 158         eventData.adjustmentFunction = adjustmentFunction;
 159         eventData.newValue = newValue;
 160         eventData.floatFlag = false;
 161         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 162     }
 163 #endif
 164 }
 165
 166 #if defined(USE_AUTOTUNE_FIXED_WING)
 167
 168 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRate, float reachedRate, float pidOutput)
 169 {
 170     float maxRateSetting = tuneCurrent[axis].rate;
 171     float gainFF = tuneCurrent[axis].gainFF;
 172     float maxDesiredRate = maxRateSetting;
 173
 174     const float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit;
 175     const float absDesiredRate = fabsf(desiredRate);
 176     const float absReachedRate = fabsf(reachedRate);
 177     const float absPidOutput = fabsf(pidOutput);
 178     const bool correctDirection = (desiredRate>0) == (reachedRate>0);
 179     float rateFullStick;
 180
 181     bool gainsUpdated = false;
 182     bool ratesUpdated = false;
 183
 184     const timeMs_t currentTimeMs = millis();
 185     static timeMs_t previousSampleTimeMs = 0;
 186     const timeDelta_t timeSincePreviousSample = currentTimeMs - previousSampleTimeMs;
 187
 188     // Use different max rate in ANLGE mode
 189     if (FLIGHT_MODE(ANGLE_MODE)) {
 190         float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
 191         maxDesiredRate = MIN(maxRateSetting, maxDesiredRateInAngleMode);
 192     }
 193
 194     const float stickInput = absDesiredRate / maxDesiredRate;
 195
 196     if ((stickInput > (pidAutotuneConfig()->fw_min_stick / 100.0f)) && correctDirection && (timeSincePreviousSample >= AUTOTUNE_FIXED_WING_SAMPLE_INTERVAL)) {
 197         // Record values every 20ms and calculate moving average over samples
 198         tuneCurrent[axis].updateCount++;
 199         tuneCurrent[axis].absDesiredRateAccum += (absDesiredRate - tuneCurrent[axis].absDesiredRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 200         tuneCurrent[axis].absReachedRateAccum += (absReachedRate - tuneCurrent[axis].absReachedRateAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 201         tuneCurrent[axis].absPidOutputAccum += (absPidOutput - tuneCurrent[axis].absPidOutputAccum) / MIN(tuneCurrent[axis].updateCount, (uint32_t)AUTOTUNE_FIXED_WING_SAMPLES);
 202
 203         if ((tuneCurrent[axis].updateCount & 25) == 0 && tuneCurrent[axis].updateCount >= AUTOTUNE_FIXED_WING_MIN_SAMPLES) {
 204             if (pidAutotuneConfig()->fw_rate_adjustment != FIXED  && !FLIGHT_MODE(ANGLE_MODE)) { // Rate discovery is not possible in ANGLE mode
 205
 206                 // Target 80% control surface deflection to leave some room for P and I to work
 207                 float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit;
 208
 209                 // Theoretically achievable rate with target deflection
 210                 rateFullStick = pidSumTarget / tuneCurrent[axis].absPidOutputAccum * tuneCurrent[axis].absReachedRateAccum;
 211
 212                 // Rate update
 213                 if (rateFullStick > (maxRateSetting + 10.0f)) {
 214                     maxRateSetting += 10.0f;
 215                 } else if (rateFullStick < (maxRateSetting - 10.0f)) {
 216                     maxRateSetting -= 10.0f;
 217                 }
 218
 219                 // Constrain to safe values
 220                 uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE;
 221                 uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate);
 222                 tuneCurrent[axis].rate = constrainf(maxRateSetting, minRate, maxRate);
 223                 ratesUpdated = true;
 224             }
 225
 226             // Update FF towards value needed to achieve current rate target
 227             gainFF += (tuneCurrent[axis].absPidOutputAccum / tuneCurrent[axis].absReachedRateAccum * FP_PID_RATE_FF_MULTIPLIER - gainFF) * (AUTOTUNE_FIXED_WING_CONVERGENCE_RATE / 100.0f);
 228             tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF);
 229             gainsUpdated = true;
 230         }
 231
 232         // Reset timer
 233         previousSampleTimeMs = currentTimeMs;
 234     }
 235
 236     if (gainsUpdated) {
 237         autotuneUpdateGains(tuneCurrent);
 238
 239         switch (axis) {
 240             case FD_ROLL:
 241                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF);
 242                 break;
 243
 244             case FD_PITCH:
 245                 blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF);
 246                 break;
 247
 248             case FD_YAW:
 249                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF);
 250                 break;
 251         }
 252         gainsUpdated = false;
 253     }
 254
 255     if (ratesUpdated) {
 256         autotuneUpdateGains(tuneCurrent);
 257
 258         switch (axis) {
 259             case FD_ROLL:
 260                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate);
 261                 break;
 262
 263             case FD_PITCH:
 264                 blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate);
 265                 break;
 266
 267             case FD_YAW:
 268                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate);
 269                 break;
 270         }
 271
 272         // Debug
 273         DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick);
 274         ratesUpdated = false;
 275     }
 276 }
 277 #endif
 278
 279 #endif