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 "common/axis.h"
  29 #include "common/maths.h"
  30 #include "common/utils.h"
  31
  32 #include "drivers/time.h"
  33
  34 #include "config/feature.h"
  35 #include "config/parameter_group.h"
  36 #include "config/parameter_group_ids.h"
  37
  38 #include "fc/config.h"
  39 #include "fc/controlrate_profile.h"
  40 #include "fc/rc_controls.h"
  41 #include "fc/rc_adjustments.h"
  42 #include "fc/runtime_config.h"
  43
  44 #include "flight/pid.h"
  45
  46 #define AUTOTUNE_FIXED_WING_OVERSHOOT_TIME      100
  47 #define AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME     200
  48 #define AUTOTUNE_FIXED_WING_INTEGRATOR_TC       600
  49 #define AUTOTUNE_FIXED_WING_DECREASE_STEP       8           // 8%
  50 #define AUTOTUNE_FIXED_WING_INCREASE_STEP       5           // 5%
  51 #define AUTOTUNE_FIXED_WING_MIN_FF              10
  52 #define AUTOTUNE_FIXED_WING_MAX_FF              200
  53
  54 PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0);
  55
  56 PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig,
  57     .fw_overshoot_time = AUTOTUNE_FIXED_WING_OVERSHOOT_TIME,
  58     .fw_undershoot_time = AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME,
  59     .fw_max_rate_threshold = 50,
  60     .fw_ff_to_p_gain = 10,
  61     .fw_ff_to_i_time_constant = AUTOTUNE_FIXED_WING_INTEGRATOR_TC,
  62 );
  63
  64 typedef enum {
  65     DEMAND_TOO_LOW,
  66     DEMAND_UNDERSHOOT,
  67     DEMAND_OVERSHOOT,
  68 } pidAutotuneState_e;
  69
  70 typedef struct {
  71     pidAutotuneState_e  state;
  72     timeMs_t            stateEnterTime;
  73
  74     bool    pidSaturated;
  75     float   gainP;
  76     float   gainI;
  77     float   gainFF;
  78 } pidAutotuneData_t;
  79
  80 #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
  81
  82 #if defined(USE_AUTOTUNE_FIXED_WING) || defined(USE_AUTOTUNE_MULTIROTOR)
  83
  84 static pidAutotuneData_t    tuneCurrent[XYZ_AXIS_COUNT];
  85 static pidAutotuneData_t    tuneSaved[XYZ_AXIS_COUNT];
  86 static timeMs_t             lastGainsUpdateTime;
  87
  88 void autotuneUpdateGains(pidAutotuneData_t * data)
  89 {
  90     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
  91         pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP);
  92         pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI);
  93         pidBankMutable()->pid[axis].D = 0;
  94         pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF);
  95     }
  96     schedulePidGainsUpdate();
  97 }
  98
  99 void autotuneCheckUpdateGains(void)
 100 {
 101     const timeMs_t currentTimeMs = millis();
 102
 103     if ((currentTimeMs - lastGainsUpdateTime) < AUTOTUNE_SAVE_PERIOD) {
 104         return;
 105     }
 106
 107     // If pilot will exit autotune we'll restore values we are flying now
 108     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 109     autotuneUpdateGains(tuneSaved);
 110     lastGainsUpdateTime = currentTimeMs;
 111 }
 112
 113 void autotuneStart(void)
 114 {
 115     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
 116         tuneCurrent[axis].gainP = pidBank()->pid[axis].P;
 117         tuneCurrent[axis].gainI = pidBank()->pid[axis].I;
 118         tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF;
 119         tuneCurrent[axis].pidSaturated = false;
 120         tuneCurrent[axis].stateEnterTime = millis();
 121         tuneCurrent[axis].state = DEMAND_TOO_LOW;
 122     }
 123
 124     memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT);
 125     lastGainsUpdateTime = millis();
 126 }
 127
 128 void autotuneUpdateState(void)
 129 {
 130     if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && ARMING_FLAG(ARMED)) {
 131         if (!FLIGHT_MODE(AUTO_TUNE)) {
 132             autotuneStart();
 133             ENABLE_FLIGHT_MODE(AUTO_TUNE);
 134         }
 135         else {
 136             autotuneCheckUpdateGains();
 137         }
 138     } else {
 139         if (FLIGHT_MODE(AUTO_TUNE)) {
 140             autotuneUpdateGains(tuneSaved);
 141         }
 142
 143         DISABLE_FLIGHT_MODE(AUTO_TUNE);
 144     }
 145 }
 146
 147 static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, int32_t newValue)
 148 {
 149 #ifndef USE_BLACKBOX
 150     UNUSED(adjustmentFunction);
 151     UNUSED(newValue);
 152 #else
 153     if (feature(FEATURE_BLACKBOX)) {
 154         flightLogEvent_inflightAdjustment_t eventData;
 155         eventData.adjustmentFunction = adjustmentFunction;
 156         eventData.newValue = newValue;
 157         eventData.floatFlag = false;
 158         blackboxLogEvent(FLIGHT_LOG_EVENT_INFLIGHT_ADJUSTMENT, (flightLogEventData_t*)&eventData);
 159     }
 160 #endif
 161 }
 162
 163 #if defined(USE_AUTOTUNE_FIXED_WING)
 164
 165 void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput)
 166 {
 167     const timeMs_t currentTimeMs = millis();
 168     const float absDesiredRateDps = fabsf(desiredRateDps);
 169     float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f;
 170     pidAutotuneState_e newState;
 171
 172     // Use different max desired rate in ANGLE for pitch and roll
 173     // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet.
 174     if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) {
 175         float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER;
 176         maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode);
 177     }
 178
 179     if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) {
 180         // If we have saturated the pid output by P+FF don't increase the gain
 181         tuneCurrent[axis].pidSaturated = true;
 182     }
 183
 184     if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) {
 185         // We can make decisions only when we are demanding at least 50% of max configured rate
 186         newState = DEMAND_TOO_LOW;
 187     }
 188     else if (fabsf(reachedRateDps) > absDesiredRateDps) {
 189         newState = DEMAND_OVERSHOOT;
 190     }
 191     else {
 192         newState = DEMAND_UNDERSHOOT;
 193     }
 194
 195     if (newState != tuneCurrent[axis].state) {
 196         const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime;
 197         bool gainsUpdated = false;
 198
 199         switch (tuneCurrent[axis].state) {
 200             case DEMAND_TOO_LOW:
 201                 break;
 202             case DEMAND_OVERSHOOT:
 203                 if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) {
 204                     tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f;
 205                     if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) {
 206                         tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF;
 207                     }
 208                     gainsUpdated = true;
 209                 }
 210                 break;
 211             case DEMAND_UNDERSHOOT:
 212                 if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) {
 213                     tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f;
 214                     if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) {
 215                         tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF;
 216                     }
 217                     gainsUpdated = true;
 218                 }
 219                 break;
 220         }
 221
 222         if (gainsUpdated) {
 223             // Set P-gain to 10% of FF gain (quite agressive - FIXME)
 224             tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f);
 225
 226             // Set integrator gain to reach the same response as FF gain in 0.667 second
 227             tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER;
 228             tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f);
 229             autotuneUpdateGains(tuneCurrent);
 230
 231             switch (axis) {
 232             case FD_ROLL:
 233                 blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainFF);
 234                 break;
 235
 236             case FD_PITCH:
 237                 blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainFF);
 238                 break;
 239
 240             case FD_YAW:
 241                 blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainFF);
 242                 break;
 243             }
 244         }
 245
 246         // Change state and reset saturation flag
 247         tuneCurrent[axis].state = newState;
 248         tuneCurrent[axis].stateEnterTime = currentTimeMs;
 249         tuneCurrent[axis].pidSaturated = false;
 250     }
 251 }
 252 #endif
 253
 254 #endif