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