src/main/flight/power_limits.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV 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 INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #if defined(USE_POWER_LIMITS)
  25
  26 #include "flight/power_limits.h"
  27
  28 #include "build/debug.h"
  29
  30 #include "common/filter.h"
  31 #include "common/maths.h"
  32 #include "common/utils.h"
  33
  34 #include "config/parameter_group.h"
  35 #include "config/parameter_group_ids.h"
  36
  37 #include "drivers/time.h"
  38
  39 #include "fc/settings.h"
  40
  41 #include "rx/rx.h"
  42
  43 #include "sensors/battery.h"
  44
  45 #define LIMITING_THR_FILTER_TCONST 50
  46
  47 PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 1);
  48
  49 PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
  50     .piP = SETTING_LIMIT_PI_P_DEFAULT,
  51     .piI = SETTING_LIMIT_PI_I_DEFAULT,
  52     .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT,   // Hz
  53 );
  54
  55 static float burstCurrentReserve;               // cA.µs
  56 static float burstCurrentReserveMax;            // cA.µs
  57 static float burstCurrentReserveFalldown;       // cA.µs
  58 static int32_t activeCurrentLimit;              // cA
  59 static float currentThrAttnIntegrator = 0;
  60 static pt1Filter_t currentThrAttnFilter;
  61 static pt1Filter_t currentThrLimitingBaseFilter;
  62 static bool wasLimitingCurrent = false;
  63
  64 #ifdef USE_ADC
  65 static float burstPowerReserve;                 // cW.µs
  66 static float burstPowerReserveMax;              // cW.µs
  67 static float burstPowerReserveFalldown;         // cW.µs
  68 static int32_t activePowerLimit;                // cW
  69 static float powerThrAttnIntegrator = 0;
  70 static pt1Filter_t powerThrAttnFilter;
  71 static pt1Filter_t powerThrLimitingBaseFilter;
  72 static bool wasLimitingPower = false;
  73 #endif
  74
  75 void powerLimiterInit(void) {
  76     if (currentBatteryProfile->powerLimits.burstCurrent < currentBatteryProfile->powerLimits.continuousCurrent) {
  77         currentBatteryProfileMutable->powerLimits.burstCurrent = currentBatteryProfile->powerLimits.continuousCurrent;
  78     }
  79
  80     activeCurrentLimit = currentBatteryProfile->powerLimits.burstCurrent;
  81
  82     uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
  83     burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
  84     burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
  85
  86     pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
  87     pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
  88
  89 #ifdef USE_ADC
  90     if (currentBatteryProfile->powerLimits.burstPower < currentBatteryProfile->powerLimits.continuousPower) {
  91         currentBatteryProfileMutable->powerLimits.burstPower = currentBatteryProfile->powerLimits.continuousPower;
  92     }
  93
  94     activePowerLimit = currentBatteryProfile->powerLimits.burstPower;
  95
  96     uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
  97     burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
  98     burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
  99
 100     pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
 101     pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
 102 #endif
 103 }
 104
 105 static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, uint32_t burstLimit, float *burstReserve, float burstReserveFalldown, float burstReserveMax, timeDelta_t timeDelta) {
 106     int32_t continuousDiff = value - continuousLimit * 10;
 107     float spentReserveChunk = continuousDiff * timeDelta;
 108     *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
 109
 110     if (currentBatteryProfile->powerLimits.burstCurrentFalldownTime) {
 111         return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
 112     }
 113
 114     return (*burstReserve ? burstLimit : continuousLimit) * 10;
 115 }
 116
 117 void currentLimiterUpdate(timeDelta_t timeDelta) {
 118     activeCurrentLimit = calculateActiveLimit(getAmperage(),
 119                             currentBatteryProfile->powerLimits.continuousCurrent, currentBatteryProfile->powerLimits.burstCurrent,
 120                             &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
 121                             timeDelta);
 122 }
 123
 124 #ifdef USE_ADC
 125 void powerLimiterUpdate(timeDelta_t timeDelta) {
 126     activePowerLimit = calculateActiveLimit(getPower(),
 127                             currentBatteryProfile->powerLimits.continuousPower, currentBatteryProfile->powerLimits.burstPower,
 128                             &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
 129                             timeDelta);
 130 }
 131 #endif
 132
 133 void powerLimiterApply(int16_t *throttleCommand) {
 134
 135 #ifdef USE_ADC
 136     if (!activeCurrentLimit && !activePowerLimit) {
 137         return;
 138     }
 139 #else
 140     if (!activeCurrentLimit) {
 141         return;
 142     }
 143 #endif
 144
 145     static timeUs_t lastCallTimestamp = 0;
 146     timeUs_t currentTimeUs = micros();
 147     timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp);
 148
 149     int16_t throttleBase;
 150     int16_t currentThrottleCommand;
 151 #ifdef USE_ADC
 152     int16_t powerThrottleCommand;
 153 #endif
 154
 155     int16_t current = getAmperageSample();
 156 #ifdef USE_ADC
 157     uint16_t voltage = getVBatSample();
 158     int32_t power = (int32_t)voltage * current / 100;
 159 #endif
 160
 161     // Current limiting
 162     int32_t overCurrent = current - activeCurrentLimit;
 163
 164     if (lastCallTimestamp) {
 165         currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
 166     }
 167
 168     float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f;
 169
 170     uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f));
 171
 172     throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand;
 173     uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
 174
 175     if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
 176         if (!wasLimitingCurrent && getAmperage() >= activeCurrentLimit) {
 177             pt1FilterReset(&currentThrLimitingBaseFilter, *throttleCommand);
 178             wasLimitingCurrent = true;
 179         }
 180
 181         currentThrottleCommand = currentThrAttned;
 182     } else {
 183         wasLimitingCurrent = false;
 184         pt1FilterReset(&currentThrAttnFilter, 0);
 185
 186         currentThrottleCommand = *throttleCommand;
 187     }
 188
 189 #ifdef USE_ADC
 190     // Power limiting
 191     int32_t overPower = power - activePowerLimit;
 192
 193     if (lastCallTimestamp) {
 194         powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5f, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
 195     }
 196
 197     float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f;
 198
 199     uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f));
 200
 201     throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
 202     uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
 203
 204     if (activePowerLimit && powerThrAttned < *throttleCommand) {
 205         if (!wasLimitingPower && getPower() >= activePowerLimit) {
 206             pt1FilterReset(&powerThrLimitingBaseFilter, *throttleCommand);
 207             wasLimitingPower = true;
 208         }
 209
 210         powerThrottleCommand = powerThrAttned;
 211     } else {
 212         wasLimitingPower = false;
 213         pt1FilterReset(&powerThrAttnFilter, 0);
 214
 215         powerThrottleCommand = *throttleCommand;
 216     }
 217
 218     *throttleCommand = MIN(currentThrottleCommand, powerThrottleCommand);
 219 #else
 220     *throttleCommand = currentThrottleCommand;
 221 #endif
 222
 223     lastCallTimestamp = currentTimeUs;
 224 }
 225
 226 bool powerLimiterIsLimiting(void) {
 227 #ifdef USE_ADC
 228     return wasLimitingPower || wasLimitingCurrent;
 229 #else
 230     return wasLimitingCurrent;
 231 #endif
 232 }
 233
 234 bool powerLimiterIsLimitingCurrent(void) {
 235     return wasLimitingCurrent;
 236 }
 237
 238 #ifdef USE_ADC
 239 bool powerLimiterIsLimitingPower(void) {
 240     return wasLimitingPower;
 241 }
 242 #endif
 243
 244 // returns seconds
 245 float powerLimiterGetRemainingBurstTime(void) {
 246     uint16_t currentBurstOverContinuous = currentBatteryProfile->powerLimits.burstCurrent - currentBatteryProfile->powerLimits.continuousCurrent;
 247     float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7f;
 248
 249 #ifdef USE_ADC
 250     uint16_t powerBurstOverContinuous = currentBatteryProfile->powerLimits.burstPower - currentBatteryProfile->powerLimits.continuousPower;
 251     float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7f;
 252
 253     if (!currentBatteryProfile->powerLimits.continuousCurrent) {
 254         return remainingPowerBurstTime;
 255     }
 256
 257     if (!currentBatteryProfile->powerLimits.continuousPower) {
 258         return remainingCurrentBurstTime;
 259     }
 260
 261     return MIN(remainingCurrentBurstTime, remainingPowerBurstTime);
 262 #else
 263     return remainingCurrentBurstTime;
 264 #endif
 265 }
 266
 267 // returns cA
 268 uint16_t powerLimiterGetActiveCurrentLimit(void) {
 269     return activeCurrentLimit;
 270 }
 271
 272 #ifdef USE_ADC
 273 // returns cW
 274 uint16_t powerLimiterGetActivePowerLimit(void) {
 275     return activePowerLimit;
 276 }
 277 #endif
 278
 279 #endif