Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

Add power limits (#6929)

Browse source 
* Add power limits

* condition USE_ADC

* Minor changes

* Increase max burst time to 5m
This commit is contained in:
Michel Pastor 2021-05-08 21:57:01 +02:00 • committed by GitHub
parent e2052a7c8f
commit e5888089a8
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
15 changed files with 557 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

11
docs/Settings.md
View file

@ -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. |

2
src/main/CMakeLists.txt
View file

@ -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

8
src/main/cms/cms_menu_osd.c
View file

@ -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,
};

4
src/main/config/parameter_group_ids.h
View file

@ -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

5
src/main/fc/fc_core.c
View file

@ -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);

7
src/main/fc/fc_init.c
View file

@ -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);

24
src/main/fc/fc_tasks.c
View file

@ -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/secondary_imu.h"
#include "flight/power_limits.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;
}

65
src/main/fc/settings.yaml
View file

@ -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

292
src/main/flight/power_limits.c Normal file
View file

@ -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

66
src/main/flight/power_limits.h Normal file
View file

@ -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

61
src/main/io/osd.c
View file

@ -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;

3
src/main/io/osd.h
View file

@ -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;

23
src/main/sensors/battery.c
View file

@ -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
// 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);
vbat = getVBatSample();
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 = microvolts / batteryMetersConfig()->current.scale; // current in 0.01A steps
amperage = pt1FilterApply4(&amperageFilterState, amperage, AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, timeDelta * 1e-6f);
break;
}
case CURRENT_SENSOR_VIRTUAL:

2
src/main/sensors/battery.h
View file

@ -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);

2
src/main/target/common.h
View file

@ -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