Add power limits (#6929)

* Add power limits

* condition USE_ADC

* Minor changes

* Increase max burst time to 5m

docs/Settings.md

@@ -214,6 +214,17 @@
 | inav_w_z_surface_v | 6.1 | 0 | 100 |  |
 | iterm_windup | 50 | 0 | 90 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) |
 | ledstrip_visual_beeper | OFF |  |  |  |
+| limit_attn_filter_cutoff | 1.2 |  | 100 | Throttle attenuation PI control output filter cutoff frequency |
+| limit_burst_current | 0 |  | 4000 | Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable |
+| limit_burst_current_falldown_time | 0 |  | 3000 | Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current` |
+| limit_burst_current_time | 0 |  | 3000 | Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced |
+| limit_burst_power | 0 |  | 40000 | Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable |
+| limit_burst_power_falldown_time | 0 |  | 3000 | Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power` |
+| limit_burst_power_time | 0 |  | 3000 | Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced |
+| limit_cont_current | 0 |  | 4000 | Continous current limit (dA), set to 0 to disable |
+| limit_cont_power | 0 |  | 40000 | Continous power limit (dW), set to 0 to disable |
+| limit_pi_i | 100 |  | 10000 | Throttle attenuation PI control I term |
+| limit_pi_p | 100 |  | 10000 | Throttle attenuation PI control P term |
 | log_level | ERROR |  |  | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. |
 | log_topics | 0 | 0 | 4294967295 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. |
 | looptime | 1000 |  | 9000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. |

src/main/CMakeLists.txt

@@ -327,6 +327,8 @@ main_sources(COMMON_SRC
     flight/pid.c
     flight/pid.h
     flight/pid_autotune.c
+    flight/power_limits.c
+    flight/power_limits.h
     flight/rth_estimator.c
     flight/rth_estimator.h
     flight/servos.c

src/main/cms/cms_menu_osd.c

@@ -316,6 +316,14 @@ static const OSD_Entry menuOsdElemsEntries[] =
     OSD_ELEMENT_ENTRY("ESC TEMPERATURE", OSD_ESC_TEMPERATURE),
 #endif
 
+#ifdef USE_POWER_LIMITS
+    OSD_ELEMENT_ENTRY("PLIM BURST TIME", OSD_PLIMIT_REMAINING_BURST_TIME),
+    OSD_ELEMENT_ENTRY("PLIM CURR LIMIT", OSD_PLIMIT_ACTIVE_CURRENT_LIMIT),
+#ifdef USE_ADC
+    OSD_ELEMENT_ENTRY("PLIM POWER LIMIT", OSD_PLIMIT_ACTIVE_POWER_LIMIT),
+#endif // USE_ADC
+#endif // USE_POWER_LIMITS
+
     OSD_BACK_AND_END_ENTRY,
 };
 

src/main/config/parameter_group_ids.h

@@ -117,7 +117,8 @@
 #define PG_DJI_OSD_CONFIG 1027
 #define PG_PROGRAMMING_PID 1028
 #define PG_SECONDARY_IMU 1029
-#define PG_INAV_END 1029
+#define PG_POWER_LIMITS_CONFIG 1030
+#define PG_INAV_END 1030
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047
@@ -132,4 +133,3 @@
 #define PG_ID_INVALID   0
 #define PG_ID_FIRST     PG_CF_START
 #define PG_ID_LAST      PG_INAV_END
-

src/main/fc/fc_core.c

@@ -88,6 +88,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/imu.h"
 
 #include "flight/failsafe.h"
+#include "flight/power_limits.h"
 
 #include "config/feature.h"
 #include "common/vector.h"
@@ -892,6 +893,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
         // FIXME: throttle pitch comp for FW
     }
 
+#ifdef USE_POWER_LIMITS
+    powerLimiterApply(&rcCommand[THROTTLE]);
+#endif
+
     // Calculate stabilisation
     pidController(dT);
 

src/main/fc/fc_init.c

@@ -97,8 +97,9 @@
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/servos.h"
+#include "flight/power_limits.h"
 #include "flight/rpm_filter.h"
+#include "flight/servos.h"
 #include "flight/secondary_imu.h"
 
 #include "io/asyncfatfs/asyncfatfs.h"
@@ -709,6 +710,10 @@ void init(void)
     ioPortExpanderInit();
 #endif
 
+#ifdef USE_POWER_LIMITS
+    powerLimiterInit();
+#endif
+
     // Considering that the persistent reset reason is only used during init
     persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_NONE);
 

src/main/fc/fc_tasks.c

@@ -43,14 +43,15 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
+#include "flight/dynamic_lpf.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
-#include "flight/wind_estimator.h"
+#include "flight/power_limits.h"
-#include "flight/secondary_imu.h"
 #include "flight/rpm_filter.h"
+#include "flight/secondary_imu.h"
 #include "flight/servos.h"
-#include "flight/dynamic_lpf.h"
+#include "flight/wind_estimator.h"
 
 #include "navigation/navigation.h"
 
@@ -112,18 +113,29 @@ void taskHandleSerial(timeUs_t currentTimeUs)
 void taskUpdateBattery(timeUs_t currentTimeUs)
 {
     static timeUs_t batMonitoringLastServiced = 0;
-    timeUs_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced);
+    timeDelta_t BatMonitoringTimeSinceLastServiced = cmpTimeUs(currentTimeUs, batMonitoringLastServiced);
 
-    if (isAmperageConfigured())
+    if (isAmperageConfigured()) {
         currentMeterUpdate(BatMonitoringTimeSinceLastServiced);
+#ifdef USE_POWER_LIMITS
+        currentLimiterUpdate(BatMonitoringTimeSinceLastServiced);
+#endif
+    }
+
 #ifdef USE_ADC
-    if (feature(FEATURE_VBAT))
+    if (feature(FEATURE_VBAT)) {
         batteryUpdate(BatMonitoringTimeSinceLastServiced);
+    }
+
     if (feature(FEATURE_VBAT) && isAmperageConfigured()) {
         powerMeterUpdate(BatMonitoringTimeSinceLastServiced);
         sagCompensatedVBatUpdate(currentTimeUs, BatMonitoringTimeSinceLastServiced);
+#if defined(USE_POWER_LIMITS) && defined(USE_ADC)
+        powerLimiterUpdate(BatMonitoringTimeSinceLastServiced);
+#endif
     }
 #endif
+
     batMonitoringLastServiced = currentTimeUs;
 }
 

src/main/fc/settings.yaml

@@ -3613,3 +3613,68 @@ groups:
         default_value: 1
         field: dshot_beeper_tone
         type: uint8_t
+
+  - name: PG_POWER_LIMITS_CONFIG
+    type: powerLimitsConfig_t
+    headers: ["flight/power_limits.h"]
+    condition: USE_POWER_LIMITS
+    members:
+      - name: limit_cont_current
+        description: "Continous current limit (dA), set to 0 to disable"
+        default_value: 0
+        field: continuousCurrent
+        max: 4000
+      - name: limit_burst_current
+        description: "Burst current limit (dA): the current which is allowed during `limit_burst_current_time` after which `limit_cont_current` will be enforced, set to 0 to disable"
+        default_value: 0
+        field: burstCurrent
+        max: 4000
+      - name: limit_burst_current_time
+        description: "Allowed current burst time (ds) during which `limit_burst_current` is allowed and after which `limit_cont_current` will be enforced"
+        default_value: 0
+        field: burstCurrentTime
+        max: 3000
+      - name: limit_burst_current_falldown_time
+        description: "Time slice at the end of the burst time during which the current limit will be ramped down from `limit_burst_current` back down to `limit_cont_current`"
+        default_value: 0
+        field: burstCurrentFalldownTime
+        max: 3000
+      - name: limit_cont_power
+        condition: USE_ADC
+        description: "Continous power limit (dW), set to 0 to disable"
+        default_value: 0
+        field: continuousPower
+        max: 40000
+      - name: limit_burst_power
+        condition: USE_ADC
+        description: "Burst power limit (dW): the current which is allowed during `limit_burst_power_time` after which `limit_cont_power` will be enforced, set to 0 to disable"
+        default_value: 0
+        field: burstPower
+        max: 40000
+      - name: limit_burst_power_time
+        condition: USE_ADC
+        description: "Allowed power burst time (ds) during which `limit_burst_power` is allowed and after which `limit_cont_power` will be enforced"
+        default_value: 0
+        field: burstPowerTime
+        max: 3000
+      - name: limit_burst_power_falldown_time
+        condition: USE_ADC
+        description: "Time slice at the end of the burst time during which the power limit will be ramped down from `limit_burst_power` back down to `limit_cont_power`"
+        default_value: 0
+        field: burstPowerFalldownTime
+        max: 3000
+      - name: limit_pi_p
+        description: "Throttle attenuation PI control P term"
+        default_value: 100
+        field: piP
+        max: 10000
+      - name: limit_pi_i
+        description: "Throttle attenuation PI control I term"
+        default_value: 100
+        field: piI
+        max: 10000
+      - name: limit_attn_filter_cutoff
+        description: "Throttle attenuation PI control output filter cutoff frequency"
+        default_value: 1.2
+        field: attnFilterCutoff
+        max: 100

src/main/flight/power_limits.c

@@ -0,0 +1,292 @@
+/*
+ * This file is part of iNav.
+ *
+ * iNav is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * iNav is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with iNav.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <math.h>
+
+#include "platform.h"
+FILE_COMPILE_FOR_SPEED
+
+#if defined(USE_POWER_LIMITS)
+
+#include "flight/power_limits.h"
+
+#include "build/debug.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#include "config/parameter_group.h"
+#include "config/parameter_group_ids.h"
+
+#include "drivers/time.h"
+
+#include "fc/settings.h"
+
+#include "rx/rx.h"
+
+#include "sensors/battery.h"
+
+#define LIMITING_THR_FILTER_TCONST 50
+
+PG_REGISTER_WITH_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig, PG_POWER_LIMITS_CONFIG, 0);
+
+PG_RESET_TEMPLATE(powerLimitsConfig_t, powerLimitsConfig,
+    .continuousCurrent = SETTING_LIMIT_CONT_CURRENT_DEFAULT,                            // dA
+    .burstCurrent = SETTING_LIMIT_BURST_CURRENT_DEFAULT,                                // dA
+    .burstCurrentTime = SETTING_LIMIT_BURST_CURRENT_TIME_DEFAULT,                       // dS
+    .burstCurrentFalldownTime = SETTING_LIMIT_BURST_CURRENT_FALLDOWN_TIME_DEFAULT,      // dS
+#ifdef USE_ADC
+    .continuousPower = SETTING_LIMIT_CONT_POWER_DEFAULT,                                // dW
+    .burstPower = SETTING_LIMIT_BURST_POWER_DEFAULT,                                    // dW
+    .burstPowerTime = SETTING_LIMIT_BURST_POWER_TIME_DEFAULT,                           // dS
+    .burstPowerFalldownTime = SETTING_LIMIT_BURST_POWER_FALLDOWN_TIME_DEFAULT,          // dS
+#endif
+    .piP = SETTING_LIMIT_PI_P_DEFAULT,
+    .piI = SETTING_LIMIT_PI_I_DEFAULT,
+    .attnFilterCutoff = SETTING_LIMIT_ATTN_FILTER_CUTOFF_DEFAULT,                       // Hz
+);
+
+static float burstCurrentReserve;               // cA.µs
+static float burstCurrentReserveMax;            // cA.µs
+static float burstCurrentReserveFalldown;       // cA.µs
+static int32_t activeCurrentLimit;              // cA
+static float currentThrAttnIntegrator = 0;
+static pt1Filter_t currentThrAttnFilter;
+static pt1Filter_t currentThrLimitingBaseFilter;
+static bool wasLimitingCurrent = false;
+
+#ifdef USE_ADC
+static float burstPowerReserve;                 // cW.µs
+static float burstPowerReserveMax;              // cW.µs
+static float burstPowerReserveFalldown;         // cW.µs
+static int32_t activePowerLimit;                // cW
+static float powerThrAttnIntegrator = 0;
+static pt1Filter_t powerThrAttnFilter;
+static pt1Filter_t powerThrLimitingBaseFilter;
+static bool wasLimitingPower = false;
+#endif
+
+void powerLimiterInit(void) {
+    if (powerLimitsConfig()->burstCurrent < powerLimitsConfig()->continuousCurrent) {
+        powerLimitsConfigMutable()->burstCurrent = powerLimitsConfig()->continuousCurrent;
+    }
+
+    activeCurrentLimit = powerLimitsConfig()->burstCurrent;
+
+    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
+    burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentTime * 1e6;
+    burstCurrentReserveFalldown = currentBurstOverContinuous * powerLimitsConfig()->burstCurrentFalldownTime * 1e6;
+
+    pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
+    pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+
+#ifdef USE_ADC
+    if (powerLimitsConfig()->burstPower < powerLimitsConfig()->continuousPower) {
+        powerLimitsConfigMutable()->burstPower = powerLimitsConfig()->continuousPower;
+    }
+
+    activePowerLimit = powerLimitsConfig()->burstPower;
+
+    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
+    burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * powerLimitsConfig()->burstPowerTime * 1e6;
+    burstPowerReserveFalldown = powerBurstOverContinuous * powerLimitsConfig()->burstPowerFalldownTime * 1e6;
+
+    pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
+    pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+#endif
+}
+
+static uint32_t calculateActiveLimit(int32_t value, uint32_t continuousLimit, uint32_t burstLimit, float *burstReserve, float burstReserveFalldown, float burstReserveMax, timeDelta_t timeDelta) {
+    int32_t continuousDiff = value - continuousLimit * 10;
+    float spentReserveChunk = continuousDiff * timeDelta;
+    *burstReserve = constrainf(*burstReserve - spentReserveChunk, 0, burstReserveMax);
+
+    if (powerLimitsConfig()->burstCurrentFalldownTime) {
+        return scaleRangef(MIN(*burstReserve, burstReserveFalldown), 0, burstReserveFalldown, continuousLimit, burstLimit) * 10;
+    }
+
+    return (*burstReserve ? burstLimit : continuousLimit) * 10;
+}
+
+void currentLimiterUpdate(timeDelta_t timeDelta) {
+    activeCurrentLimit = calculateActiveLimit(getAmperage(),
+                            powerLimitsConfig()->continuousCurrent, powerLimitsConfig()->burstCurrent,
+                            &burstCurrentReserve, burstCurrentReserveFalldown, burstCurrentReserveMax,
+                            timeDelta);
+}
+
+#ifdef USE_ADC
+void powerLimiterUpdate(timeDelta_t timeDelta) {
+    activePowerLimit = calculateActiveLimit(getPower(),
+                            powerLimitsConfig()->continuousPower, powerLimitsConfig()->burstPower,
+                            &burstPowerReserve, burstPowerReserveFalldown, burstPowerReserveMax,
+                            timeDelta);
+}
+#endif
+
+void powerLimiterApply(int16_t *throttleCommand) {
+
+#ifdef USE_ADC
+    if (!activeCurrentLimit && !activePowerLimit) {
+        return;
+    }
+#else
+    if (!activeCurrentLimit) {
+        return;
+    }
+#endif
+
+    static timeUs_t lastCallTimestamp = 0;
+    timeUs_t currentTimeUs = micros();
+    timeDelta_t callTimeDelta = cmpTimeUs(currentTimeUs, lastCallTimestamp);
+
+    int16_t throttleBase;
+    int16_t currentThrottleCommand;
+#ifdef USE_ADC
+    int16_t powerThrottleCommand;
+#endif
+
+    int16_t current = getAmperageSample();
+#ifdef USE_ADC
+    uint16_t voltage = getVBatSample();
+    int32_t power = (int32_t)voltage * current / 100;
+#endif
+
+    // Current limiting
+    int32_t overCurrent = current - activeCurrentLimit;
+
+    if (lastCallTimestamp) {
+        currentThrAttnIntegrator = constrainf(currentThrAttnIntegrator + overCurrent * powerLimitsConfig()->piI * callTimeDelta * 2e-7, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
+
+    float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3;
+
+    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6));
+
+    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
+
+    if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
+        if (!wasLimitingCurrent && getAmperage() >= activeCurrentLimit) {
+            pt1FilterReset(&currentThrLimitingBaseFilter, *throttleCommand);
+            wasLimitingCurrent = true;
+        }
+
+        currentThrottleCommand = currentThrAttned;
+    } else {
+        wasLimitingCurrent = false;
+        currentThrAttnIntegrator = 0;
+        pt1FilterReset(&currentThrAttnFilter, 0);
+
+        currentThrottleCommand = *throttleCommand;
+    }
+
+#ifdef USE_ADC
+    // Power limiting
+    int32_t overPower = power - activePowerLimit;
+
+    if (lastCallTimestamp) {
+        powerThrAttnIntegrator = constrainf(powerThrAttnIntegrator + overPower * powerLimitsConfig()->piI * callTimeDelta / voltage * 2e-5, 0, PWM_RANGE_MAX - PWM_RANGE_MIN);
+    }
+
+    float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1;
+
+    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6));
+
+    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
+
+    if (activePowerLimit && powerThrAttned < *throttleCommand) {
+        if (!wasLimitingPower && getPower() >= activePowerLimit) {
+            pt1FilterReset(&powerThrLimitingBaseFilter, *throttleCommand);
+            wasLimitingPower = true;
+        }
+
+        powerThrottleCommand = powerThrAttned;
+    } else {
+        wasLimitingPower = false;
+        powerThrAttnIntegrator = 0;
+        pt1FilterReset(&powerThrAttnFilter, 0);
+
+        powerThrottleCommand = *throttleCommand;
+    }
+
+    *throttleCommand = MIN(currentThrottleCommand, powerThrottleCommand);
+#else
+    *throttleCommand = currentThrottleCommand;
+#endif
+
+    lastCallTimestamp = currentTimeUs;
+}
+
+bool powerLimiterIsLimiting(void) {
+#ifdef USE_ADC
+    return wasLimitingPower || wasLimitingCurrent;
+#else
+    return wasLimitingCurrent;
+#endif
+}
+
+bool powerLimiterIsLimitingCurrent(void) {
+    return wasLimitingCurrent;
+}
+
+#ifdef USE_ADC
+bool powerLimiterIsLimitingPower(void) {
+    return wasLimitingPower;
+}
+#endif
+
+// returns seconds
+float powerLimiterGetRemainingBurstTime(void) {
+    uint16_t currentBurstOverContinuous = powerLimitsConfig()->burstCurrent - powerLimitsConfig()->continuousCurrent;
+    float remainingCurrentBurstTime = burstCurrentReserve / currentBurstOverContinuous / 1e7;
+
+#ifdef USE_ADC
+    uint16_t powerBurstOverContinuous = powerLimitsConfig()->burstPower - powerLimitsConfig()->continuousPower;
+    float remainingPowerBurstTime = burstPowerReserve / powerBurstOverContinuous / 1e7;
+
+    if (!powerLimitsConfig()->continuousCurrent) {
+        return remainingPowerBurstTime;
+    }
+
+    if (!powerLimitsConfig()->continuousPower) {
+        return remainingCurrentBurstTime;
+    }
+
+    return MIN(remainingCurrentBurstTime, remainingPowerBurstTime);
+#else
+    return remainingCurrentBurstTime;
+#endif
+}
+
+// returns cA
+uint16_t powerLimiterGetActiveCurrentLimit(void) {
+    return activeCurrentLimit;
+}
+
+#ifdef USE_ADC
+// returns cW
+uint16_t powerLimiterGetActivePowerLimit(void) {
+    return activePowerLimit;
+}
+#endif
+
+#endif

src/main/flight/power_limits.h

@@ -0,0 +1,66 @@
+/*
+ * This file is part of iNav
+ *
+ * iNav free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * iNav distributed in the hope that it
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <common/time.h>
+
+#include "config/parameter_group.h"
+
+#if defined(USE_POWER_LIMITS)
+
+typedef struct {
+    uint16_t continuousCurrent;         // dA
+    uint16_t burstCurrent;              // dA
+    uint16_t burstCurrentTime;          // ds
+    uint16_t burstCurrentFalldownTime;  // ds
+
+#ifdef USE_ADC
+    uint16_t continuousPower;           // dW
+    uint16_t burstPower;                // dW
+    uint16_t burstPowerTime;            // ds
+    uint16_t burstPowerFalldownTime;    // ds
+#endif
+
+    uint16_t piP;
+    uint16_t piI;
+
+    float attnFilterCutoff;             // Hz
+} powerLimitsConfig_t;
+
+PG_DECLARE(powerLimitsConfig_t, powerLimitsConfig);
+
+void powerLimiterInit(void);
+void currentLimiterUpdate(timeDelta_t timeDelta);
+void powerLimiterUpdate(timeDelta_t timeDelta);
+void powerLimiterApply(int16_t *throttleCommand);
+bool powerLimiterIsLimiting(void);
+bool powerLimiterIsLimitingCurrent(void);
+float powerLimiterGetRemainingBurstTime(void);      // returns seconds
+uint16_t powerLimiterGetActiveCurrentLimit(void);   // returns cA
+#ifdef USE_ADC
+uint16_t powerLimiterGetActivePowerLimit(void);     // returns cW
+bool powerLimiterIsLimitingPower(void);
+#endif
+
+#endif

src/main/io/osd.c

@@ -85,10 +85,11 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
+#include "flight/power_limits.h"
 #include "flight/rth_estimator.h"
-#include "flight/wind_estimator.h"
 #include "flight/secondary_imu.h"
 #include "flight/servos.h"
+#include "flight/wind_estimator.h"
 
 #include "navigation/navigation.h"
 #include "navigation/navigation_private.h"
@@ -915,9 +916,15 @@ static void osdFormatThrottlePosition(char *buff, bool autoThr, textAttributes_t
     if (autoThr && navigationIsControllingThrottle()) {
         buff[0] = SYM_AUTO_THR0;
         buff[1] = SYM_AUTO_THR1;
-        if (isFixedWingAutoThrottleManuallyIncreased())
+        if (isFixedWingAutoThrottleManuallyIncreased()) {
             TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
+        }
     }
+#ifdef USE_POWER_LIMITS
+    if (powerLimiterIsLimiting()) {
+        TEXT_ATTRIBUTES_ADD_BLINK(*elemAttr);
+    }
+#endif
     tfp_sprintf(buff + 2, "%3d", getThrottlePercent());
 }
 
@@ -2697,9 +2704,47 @@ static bool osdDrawSingleElement(uint8_t item)
 
             return true;
         }
+
     case OSD_NAV_FW_CONTROL_SMOOTHNESS:
         osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CTL S", 0, navConfig()->fw.control_smoothness, 1, 0, ADJUSTMENT_NAV_FW_CONTROL_SMOOTHNESS);
         return true;
+
+#ifdef USE_POWER_LIMITS
+    case OSD_PLIMIT_REMAINING_BURST_TIME:
+        osdFormatCentiNumber(buff, powerLimiterGetRemainingBurstTime() * 100, 0, 1, 0, 3);
+        buff[3] = 'S';
+        buff[4] = '\0';
+        break;
+
+    case OSD_PLIMIT_ACTIVE_CURRENT_LIMIT:
+        if (powerLimitsConfig()->continuousCurrent) {
+            osdFormatCentiNumber(buff, powerLimiterGetActiveCurrentLimit(), 0, 2, 0, 3);
+            buff[3] = SYM_AMP;
+            buff[4] = '\0';
+
+            if (powerLimiterIsLimitingCurrent()) {
+                TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+            }
+        }
+        break;
+
+#ifdef USE_ADC
+    case OSD_PLIMIT_ACTIVE_POWER_LIMIT:
+        {
+            if (powerLimitsConfig()->continuousPower) {
+                bool kiloWatt = osdFormatCentiNumber(buff, powerLimiterGetActivePowerLimit(), 1000, 2, 2, 3);
+                buff[3] = kiloWatt ? SYM_KILOWATT : SYM_WATT;
+                buff[4] = '\0';
+
+                if (powerLimiterIsLimitingPower()) {
+                    TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
+                }
+            }
+            break;
+        }
+#endif // USE_ADC
+#endif // USE_POWER_LIMITS
+
     default:
         return false;
     }
@@ -2774,6 +2819,12 @@ static uint8_t osdIncElementIndex(uint8_t elementIndex)
         }
     }
 
+#ifndef USE_POWER_LIMITS
+    if (elementIndex == OSD_NAV_FW_CONTROL_SMOOTHNESS) {
+        elementIndex = OSD_ITEM_COUNT;
+    }
+#endif
+
     if (elementIndex == OSD_ITEM_COUNT) {
         elementIndex = 0;
     }
@@ -3020,6 +3071,12 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
     osdLayoutsConfig->item_pos[0][OSD_RC_SOURCE] = OSD_POS(3, 4);
 #endif
 
+#ifdef USE_POWER_LIMITS
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_REMAINING_BURST_TIME] = OSD_POS(3, 4);
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_CURRENT_LIMIT] = OSD_POS(3, 5);
+    osdLayoutsConfig->item_pos[0][OSD_PLIMIT_ACTIVE_POWER_LIMIT] = OSD_POS(3, 6);
+#endif
+
     // Under OSD_FLYMODE. TODO: Might not be visible on NTSC?
     osdLayoutsConfig->item_pos[0][OSD_MESSAGES] = OSD_POS(1, 13) | OSD_VISIBLE_FLAG;
 

src/main/io/osd.h

@@ -228,6 +228,9 @@ typedef enum {
     OSD_NAV_FW_CONTROL_SMOOTHNESS,
     OSD_VERSION,
     OSD_RANGEFINDER,
+    OSD_PLIMIT_REMAINING_BURST_TIME,
+    OSD_PLIMIT_ACTIVE_CURRENT_LIMIT,
+    OSD_PLIMIT_ACTIVE_POWER_LIMIT,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;
 

src/main/sensors/battery.c

@@ -218,9 +218,7 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
     switch (batteryMetersConfig()->voltage.type) {
         case VOLTAGE_SENSOR_ADC:
             {
-                // calculate battery voltage based on ADC reading
+                vbat = getVBatSample();
-                // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
-                vbat = (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000);
                 break;
             }
 #if defined(USE_ESC_SENSOR)
@@ -375,6 +373,14 @@ bool isBatteryVoltageConfigured(void)
     return feature(FEATURE_VBAT);
 }
 
+#ifdef USE_ADC
+uint16_t getVBatSample(void) {
+    // calculate battery voltage based on ADC reading
+    // result is Vbatt in 0.01V steps. 3.3V = ADC Vref, 0xFFF = 12bit adc, 1100 = 11:1 voltage divider (10k:1k)
+    return (uint64_t)adcGetChannel(ADC_BATTERY) * batteryMetersConfig()->voltage.scale * ADCVREF / (0xFFF * 1000);
+}
+#endif
+
 uint16_t getBatteryVoltage(void)
 {
     if (batteryMetersConfig()->voltageSource == BAT_VOLTAGE_SAG_COMP) {
@@ -448,6 +454,12 @@ int16_t getAmperage(void)
     return amperage;
 }
 
+int16_t getAmperageSample(void)
+{
+    int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100;
+    return microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
+}
+
 int32_t getPower(void)
 {
     return power;
@@ -463,7 +475,6 @@ int32_t getMWhDrawn(void)
     return mWhDrawn;
 }
 
-
 void currentMeterUpdate(timeUs_t timeDelta)
 {
     static pt1Filter_t amperageFilterState;
@@ -472,9 +483,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
     switch (batteryMetersConfig()->current.type) {
         case CURRENT_SENSOR_ADC:
             {
-                int32_t microvolts = ((uint32_t)adcGetChannel(ADC_CURRENT) * ADCVREF * 100) / 0xFFF * 10 - (int32_t)batteryMetersConfig()->current.offset * 100;
+                amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
-                amperage = microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
-                amperage = pt1FilterApply4(&amperageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
                 break;
             }
         case CURRENT_SENSOR_VIRTUAL:

src/main/sensors/battery.h

@@ -151,6 +151,7 @@ uint16_t getPowerSupplyImpedance(void);
 
 bool isAmperageConfigured(void);
 int16_t getAmperage(void);
+int16_t getAmperageSample(void);
 int32_t getPower(void);
 int32_t getMAhDrawn(void);
 int32_t getMWhDrawn(void);
@@ -159,6 +160,7 @@ int32_t getMWhDrawn(void);
 void batteryUpdate(timeUs_t timeDelta);
 void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta);
 void powerMeterUpdate(timeUs_t timeDelta);
+uint16_t getVBatSample(void);
 #endif
 
 void currentMeterUpdate(timeUs_t timeDelta);

src/main/target/common.h

@@ -167,6 +167,8 @@
 #define USE_SECONDARY_IMU
 #define USE_IMU_BNO055
 
+#define USE_POWER_LIMITS
+
 #else // MCU_FLASH_SIZE < 256
 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR
 #endif