Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge pull request #9561 from ctzsnooze/vbat-motor-limit

Browse source 
Battery sag compensation by varying motor output
This commit is contained in:
Michael Keller 2020-03-26 13:58:16 +13:00 • committed by GitHub
commit 395a412b59
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 172 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/cli/settings.c
View file

@ -840,10 +840,13 @@ const clivalue_t valueTable[] = {
{ "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) },
{ "vbat_cutoff_percent", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, lvcPercentage) },
{ "force_battery_cell_count", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 24 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, forceBatteryCellCount) },
{ "vbat_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatLpfPeriod) },
{ "vbat_display_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, UINT8_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatDisplayLpfPeriod) },
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
{ "vbat_sag_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 1, UINT8_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatSagLpfPeriod) },
#endif
{ "ibat_lpf_period", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, ibatLpfPeriod) },
{ "vbat_duration_for_warning", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 150 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatDurationForWarning) },
{ "vbat_duration_for_critical", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 150 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatDurationForCritical) },
{ "vbat_duration_for_critical", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 150 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, vbatDurationForCritical) },
// PG_VOLTAGE_SENSOR_ADC_CONFIG
{ "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_VOLTAGE_SENSOR_ADC_CONFIG, offsetof(voltageSensorADCConfig_t, vbatscale) },
@ -1007,6 +1010,9 @@ const clivalue_t valueTable[] = {
{ "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_hz) },
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, FILTER_FREQUENCY_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
{ "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
{ "vbat_sag_compensation", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, vbat_sag_compensation) },
#endif
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },

6
src/main/config/config.c
View file

@ -261,6 +261,12 @@ static void validateAndFixConfig(void)
pidProfilesMutable(i)->d_min[axis] = 0;
}
}
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_ADC) {
pidProfilesMutable(i)->vbat_sag_compensation = 0;
}
#endif
}
if (motorConfig()->dev.motorPwmProtocol == PWM_TYPE_BRUSHED) {

10
src/main/fc/tasks.c
View file

@ -229,6 +229,14 @@ void tasksInit(void)
const bool useBatteryVoltage = batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE;
setTaskEnabled(TASK_BATTERY_VOLTAGE, useBatteryVoltage);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
// If vbat motor output compensation is used, use fast vbat samplingTime
if (isSagCompensationConfigured()) {
rescheduleTask(TASK_BATTERY_VOLTAGE, TASK_PERIOD_HZ(FAST_VOLTAGE_TASK_FREQ_HZ));
}
#endif
const bool useBatteryCurrent = batteryConfig()->currentMeterSource != CURRENT_METER_NONE;
setTaskEnabled(TASK_BATTERY_CURRENT, useBatteryCurrent);
const bool useBatteryAlerts = batteryConfig()->useVBatAlerts || batteryConfig()->useConsumptionAlerts || featureIsEnabled(FEATURE_OSD);
@ -378,7 +386,7 @@ task_t tasks[TASK_COUNT] = {
[TASK_MAIN] = DEFINE_TASK("SYSTEM", "UPDATE", NULL, taskMain, TASK_PERIOD_HZ(1000), TASK_PRIORITY_MEDIUM_HIGH),
[TASK_SERIAL] = DEFINE_TASK("SERIAL", NULL, NULL, taskHandleSerial, TASK_PERIOD_HZ(100), TASK_PRIORITY_LOW), // 100 Hz should be enough to flush up to 115 bytes @ 115200 baud
[TASK_BATTERY_ALERTS] = DEFINE_TASK("BATTERY_ALERTS", NULL, NULL, taskBatteryAlerts, TASK_PERIOD_HZ(5), TASK_PRIORITY_MEDIUM),
[TASK_BATTERY_VOLTAGE] = DEFINE_TASK("BATTERY_VOLTAGE", NULL, NULL, batteryUpdateVoltage, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM),
[TASK_BATTERY_VOLTAGE] = DEFINE_TASK("BATTERY_VOLTAGE", NULL, NULL, batteryUpdateVoltage, TASK_PERIOD_HZ(SLOW_VOLTAGE_TASK_FREQ_HZ), TASK_PRIORITY_MEDIUM), // Freq may be updated in tasksInit
[TASK_BATTERY_CURRENT] = DEFINE_TASK("BATTERY_CURRENT", NULL, NULL, batteryUpdateCurrentMeter, TASK_PERIOD_HZ(50), TASK_PRIORITY_MEDIUM),
#ifdef USE_TRANSPONDER

38
src/main/flight/mixer.c
View file

@ -296,7 +296,11 @@ static FAST_RAM_ZERO_INIT float idleThrottleOffset;
static FAST_RAM_ZERO_INIT float idleMinMotorRps;
static FAST_RAM_ZERO_INIT float idleP;
#endif
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
static FAST_RAM_ZERO_INIT float vbatSagCompensationFactor;
static FAST_RAM_ZERO_INIT float vbatFull;
static FAST_RAM_ZERO_INIT float vbatRangeToCompensate;
#endif
uint8_t getMotorCount(void)
{
@ -355,6 +359,18 @@ void mixerInitProfile(void)
idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f;
idleP = currentPidProfile->idle_p * 0.0001f;
#endif
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
vbatSagCompensationFactor = 0.0f;
if (currentPidProfile->vbat_sag_compensation > 0) {
//TODO: Make this voltage user configurable
vbatFull = CELL_VOLTAGE_FULL_CV;
vbatRangeToCompensate = vbatFull - batteryConfig()->vbatwarningcellvoltage;
if (vbatRangeToCompensate >= 0) {
vbatSagCompensationFactor = ((float)currentPidProfile->vbat_sag_compensation) / 100.0f;
}
}
#endif
}
void mixerInit(mixerMode_e mixerMode)
@ -623,11 +639,27 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
}
#endif
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
float motorRangeAttenuationFactor = 0;
// reduce motorRangeMax when battery is full
if (vbatSagCompensationFactor > 0.0f) {
const uint16_t currentCellVoltage = getBatterySagCellVoltage();
// batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow
float batteryGoodness = 1.0f - constrainf((vbatFull - currentCellVoltage) / vbatRangeToCompensate, 0.0f, 1.0f);
motorRangeAttenuationFactor = (vbatRangeToCompensate / vbatFull) * batteryGoodness * vbatSagCompensationFactor;
DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100);
DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000);
}
motorRangeMax = motorOutputHigh - motorRangeAttenuationFactor * (motorOutputHigh - motorOutputLow);
#else
motorRangeMax = motorOutputHigh;
#endif
currentThrottleInputRange = rcCommandThrottleRange;
motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow);
motorRangeMax = motorOutputHigh;
motorOutputMin = motorRangeMin;
motorOutputRange = motorOutputHigh - motorOutputMin;
motorOutputRange = motorRangeMax - motorRangeMin;
motorOutputMixSign = 1;
}

3
src/main/flight/pid.c
View file

@ -135,7 +135,7 @@ static FAST_RAM_ZERO_INIT float airmodeThrottleOffsetLimit;
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 14);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 15);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -224,6 +224,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.ff_boost = 15,
.dyn_lpf_curve_expo = 0,
.level_race_mode = false,
.vbat_sag_compensation = 0,
);
#ifndef USE_D_MIN
pidProfile->pid[PID_ROLL].D = 30;

1
src/main/flight/pid.h
View file

@ -190,6 +190,7 @@ typedef struct pidProfile_s {
uint8_t ff_smooth_factor; // Amount of smoothing for interpolated FF steps
uint8_t dyn_lpf_curve_expo; // set the curve for dynamic dterm lowpass filter
uint8_t level_race_mode; // NFE race mode - when true pitch setpoint calcualtion is gyro based in level mode
uint8_t vbat_sag_compensation; // Reduce motor output by this percentage of the maximum compensation amount
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);

2
src/main/msp/msp.c
View file

@ -742,7 +742,7 @@ static bool mspCommonProcessOutCommand(int16_t cmdMSP, sbuf_t *dst, mspPostProce
voltageMeterRead(id, &meter);
sbufWriteU8(dst, id);
sbufWriteU8(dst, (uint8_t)constrain((meter.filtered + 5) / 10, 0, 255));
sbufWriteU8(dst, (uint8_t)constrain((meter.displayFiltered + 5) / 10, 0, 255));
}
break;
}

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

@ -118,7 +118,8 @@ PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig,
.vbatfullcellvoltage = 410,
.vbatLpfPeriod = 30,
.vbatDisplayLpfPeriod = 30,
.vbatSagLpfPeriod = 2,
.ibatLpfPeriod = 10,
.vbatDurationForWarning = 0,
.vbatDurationForCritical = 0,
@ -148,10 +149,8 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs)
break;
}
if (debugMode == DEBUG_BATTERY) {
debug[0] = voltageMeter.unfiltered;
debug[1] = voltageMeter.filtered;
}
DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
}
static void updateBatteryBeeperAlert(void)
@ -172,18 +171,20 @@ static void updateBatteryBeeperAlert(void)
}
}
//TODO: make all of these independent of voltage filtering for display
static bool isVoltageStable(void)
{
return ABS(voltageMeter.filtered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
return ABS(voltageMeter.displayFiltered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
}
static bool isVoltageFromBat(void)
{
// We want to disable battery getting detected around USB voltage or 0V
return (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage // Above ~0V
&& voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage) // 1s max cell voltage check
|| voltageMeter.filtered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
return (voltageMeter.displayFiltered >= batteryConfig()->vbatnotpresentcellvoltage // Above ~0V
&& voltageMeter.displayFiltered <= batteryConfig()->vbatmaxcellvoltage) // 1s max cell voltage check
|| voltageMeter.displayFiltered > batteryConfig()->vbatnotpresentcellvoltage * 2; // USB voltage - 2s or more check
}
void batteryUpdatePresence(void)
@ -199,7 +200,7 @@ void batteryUpdatePresence(void)
if (batteryConfig()->forceBatteryCellCount != 0) {
batteryCellCount = batteryConfig()->forceBatteryCellCount;
} else {
unsigned cells = (voltageMeter.filtered / batteryConfig()->vbatmaxcellvoltage) + 1;
unsigned cells = (voltageMeter.displayFiltered / batteryConfig()->vbatmaxcellvoltage) + 1;
if (cells > MAX_AUTO_DETECT_CELL_COUNT) {
// something is wrong, we expect MAX_CELL_COUNT cells maximum (and autodetection will be problematic at 6+ cells)
cells = MAX_AUTO_DETECT_CELL_COUNT;
@ -225,10 +226,6 @@ void batteryUpdatePresence(void)
batteryWarningVoltage = 0;
batteryCriticalVoltage = 0;
}
if (debugMode == DEBUG_BATTERY) {
debug[2] = batteryCellCount;
debug[3] = isVoltageStable();
}
}
static void batteryUpdateVoltageState(void)
@ -237,7 +234,7 @@ static void batteryUpdateVoltageState(void)
static uint32_t lastVoltageChangeMs;
switch (voltageState) {
case BATTERY_OK:
if (voltageMeter.filtered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
if (voltageMeter.displayFiltered <= (batteryWarningVoltage - batteryConfig()->vbathysteresis)) {
if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForWarning * 100) {
voltageState = BATTERY_WARNING;
}
@ -247,12 +244,12 @@ static void batteryUpdateVoltageState(void)
break;
case BATTERY_WARNING:
if (voltageMeter.filtered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
if (voltageMeter.displayFiltered <= (batteryCriticalVoltage - batteryConfig()->vbathysteresis)) {
if (cmp32(millis(), lastVoltageChangeMs) >= batteryConfig()->vbatDurationForCritical * 100) {
voltageState = BATTERY_CRITICAL;
}
} else {
if (voltageMeter.filtered > batteryWarningVoltage) {
if (voltageMeter.displayFiltered > batteryWarningVoltage) {
voltageState = BATTERY_OK;
}
lastVoltageChangeMs = millis();
@ -260,7 +257,7 @@ static void batteryUpdateVoltageState(void)
break;
case BATTERY_CRITICAL:
if (voltageMeter.filtered > batteryCriticalVoltage) {
if (voltageMeter.displayFiltered > batteryCriticalVoltage) {
voltageState = BATTERY_WARNING;
lastVoltageChangeMs = millis();
}
@ -361,6 +358,8 @@ void batteryInit(void)
lowVoltageCutoff.startTime = 0;
voltageMeterReset(&voltageMeter);
voltageMeterGenericInit();
switch (batteryConfig()->voltageMeterSource) {
case VOLTAGE_METER_ESC:
#ifdef USE_ESC_SENSOR
@ -465,7 +464,7 @@ float calculateVbatPidCompensation(void) {
float batteryScaler = 1.0f;
if (batteryConfig()->voltageMeterSource != VOLTAGE_METER_NONE && batteryCellCount > 0) {
// Up to 33% PID gain. Should be fine for 4,2to 3,3 difference
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.filtered), 1.0f, 1.33f);
batteryScaler = constrainf((( (float)batteryConfig()->vbatmaxcellvoltage * batteryCellCount ) / (float) voltageMeter.displayFiltered), 1.0f, 1.33f);
}
return batteryScaler;
}
@ -479,7 +478,7 @@ uint8_t calculateBatteryPercentageRemaining(void)
if (batteryCapacity > 0) {
batteryPercentage = constrain(((float)batteryCapacity - currentMeter.mAhDrawn) * 100 / batteryCapacity, 0, 100);
} else {
batteryPercentage = constrain((((uint32_t)voltageMeter.filtered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
batteryPercentage = constrain((((uint32_t)voltageMeter.displayFiltered - (batteryConfig()->vbatmincellvoltage * batteryCellCount)) * 100) / ((batteryConfig()->vbatmaxcellvoltage - batteryConfig()->vbatmincellvoltage) * batteryCellCount), 0, 100);
}
}
@ -501,12 +500,12 @@ bool isBatteryVoltageConfigured(void)
uint16_t getBatteryVoltage(void)
{
return voltageMeter.filtered;
return voltageMeter.displayFiltered;
}
uint16_t getLegacyBatteryVoltage(void)
{
return (voltageMeter.filtered + 5) / 10;
return (voltageMeter.displayFiltered + 5) / 10;
}
uint16_t getBatteryVoltageLatest(void)
@ -521,9 +520,16 @@ uint8_t getBatteryCellCount(void)
uint16_t getBatteryAverageCellVoltage(void)
{
return voltageMeter.filtered / batteryCellCount;
return voltageMeter.displayFiltered / batteryCellCount;
}
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
uint16_t getBatterySagCellVoltage(void)
{
return voltageMeter.sagFiltered / batteryCellCount;
}
#endif
bool isAmperageConfigured(void)
{
return batteryConfig()->currentMeterSource != CURRENT_METER_NONE;

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

@ -27,6 +27,9 @@
#include "sensors/current.h"
#include "sensors/voltage.h"
//TODO: Make the 'cell full' voltage user adjustble
#define CELL_VOLTAGE_FULL_CV 420
#define VBAT_CELL_VOTAGE_RANGE_MIN 100
#define VBAT_CELL_VOTAGE_RANGE_MAX 500
@ -61,10 +64,11 @@ typedef struct batteryConfig_s {
uint16_t vbatfullcellvoltage; // Cell voltage at which the battery is deemed to be "full" 0.01V units, default is 410 (4.1V)
uint8_t forceBatteryCellCount; // Number of cells in battery, used for overwriting auto-detected cell count if someone has issues with it.
uint8_t vbatLpfPeriod; // Period of the cutoff frequency for the Vbat filter (in 0.1 s)
uint8_t vbatDisplayLpfPeriod; // Period of the cutoff frequency for the Vbat filter for display and startup (in 0.1 s)
uint8_t ibatLpfPeriod; // Period of the cutoff frequency for the Ibat filter (in 0.1 s)
uint8_t vbatDurationForWarning; // Period voltage has to sustain before the battery state is set to BATTERY_WARNING (in 0.1 s)
uint8_t vbatDurationForCritical; // Period voltage has to sustain before the battery state is set to BATTERY_CRIT (in 0.1 s)
uint8_t vbatDurationForWarning; // Period voltage has to sustain before the battery state is set to BATTERY_WARNING (in 0.1 s)
uint8_t vbatDurationForCritical; // Period voltage has to sustain before the battery state is set to BATTERY_CRIT (in 0.1 s)
uint8_t vbatSagLpfPeriod; // Period of the cutoff frequency for the Vbat sag and PID compensation filter (in 0.1 s)
} batteryConfig_t;
PG_DECLARE(batteryConfig_t, batteryConfig);
@ -105,6 +109,7 @@ uint16_t getLegacyBatteryVoltage(void);
uint16_t getBatteryVoltageLatest(void);
uint8_t getBatteryCellCount(void);
uint16_t getBatteryAverageCellVoltage(void);
uint16_t getBatterySagCellVoltage(void);
bool isAmperageConfigured(void);
int32_t getAmperage(void);

78
src/main/sensors/voltage.c
View file

@ -30,10 +30,13 @@
#include "common/maths.h"
#include "common/utils.h"
#include "config/config.h"
#include "config/config_reset.h"
#include "drivers/adc.h"
#include "flight/pid.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
@ -84,8 +87,11 @@ const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds);
void voltageMeterReset(voltageMeter_t *meter)
{
meter->filtered = 0;
meter->displayFiltered = 0;
meter->unfiltered = 0;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
meter->sagFiltered = 0;
#endif
}
//
// ADC
@ -104,15 +110,19 @@ void voltageMeterReset(voltageMeter_t *meter)
#endif
typedef struct voltageMeterADCState_s {
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageDisplayFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
pt1Filter_t filter;
pt1Filter_t displayFilter;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
uint16_t voltageSagFiltered; // battery voltage in 0.01V steps (filtered for vbat sag compensation)
pt1Filter_t sagFilter; // filter for vbat sag compensation
#endif
} voltageMeterADCState_t;
extern voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
voltageMeterADCState_t voltageMeterADCStates[MAX_VOLTAGE_SENSOR_ADC];
static bool sagCompensationConfigured;
voltageMeterADCState_t *getVoltageMeterADC(uint8_t index)
{
return &voltageMeterADCStates[index];
@ -163,17 +173,26 @@ void voltageMeterADCRefresh(void)
uint8_t channel = voltageMeterAdcChannelMap[i];
uint16_t rawSample = adcGetChannel(channel);
uint16_t filteredSample = pt1FilterApply(&state->filter, rawSample);
uint16_t filteredDisplaySample = pt1FilterApply(&state->displayFilter, rawSample);
// always calculate the latest voltage, see getLatestVoltage() which does the calculation on demand.
state->voltageFiltered = voltageAdcToVoltage(filteredSample, config);
state->voltageDisplayFiltered = voltageAdcToVoltage(filteredDisplaySample, config);
state->voltageUnfiltered = voltageAdcToVoltage(rawSample, config);
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
if (isSagCompensationConfigured()) {
uint16_t filteredSagSample = pt1FilterApply(&state->sagFilter, rawSample);
state->voltageSagFiltered = voltageAdcToVoltage(filteredSagSample, config);
}
#endif
#else
UNUSED(voltageAdcToVoltage);
state->voltageFiltered = 0;
state->voltageDisplayFiltered = 0;
state->voltageUnfiltered = 0;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
state->voltageSagFiltered = 0;
#endif
#endif
}
}
@ -182,8 +201,16 @@ void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageM
{
voltageMeterADCState_t *state = &voltageMeterADCStates[adcChannel];
voltageMeter->filtered = state->voltageFiltered;
voltageMeter->displayFiltered = state->voltageDisplayFiltered;
voltageMeter->unfiltered = state->voltageUnfiltered;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
voltageMeter->sagFiltered = state->voltageSagFiltered;
#endif
}
bool isSagCompensationConfigured(void)
{
return sagCompensationConfigured;
}
void voltageMeterADCInit(void)
@ -194,19 +221,36 @@ void voltageMeterADCInit(void)
voltageMeterADCState_t *state = &voltageMeterADCStates[i];
memset(state, 0, sizeof(voltageMeterADCState_t));
pt1FilterInit(&state->filter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL(50)));
pt1FilterInit(&state->displayFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatDisplayLpfPeriod), HZ_TO_INTERVAL(isSagCompensationConfigured() ? FAST_VOLTAGE_TASK_FREQ_HZ : SLOW_VOLTAGE_TASK_FREQ_HZ)));
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
if (isSagCompensationConfigured()) {
pt1FilterInit(&state->sagFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatSagLpfPeriod), HZ_TO_INTERVAL(FAST_VOLTAGE_TASK_FREQ_HZ)));
}
#endif
}
}
void voltageMeterGenericInit(void)
{
sagCompensationConfigured = false;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
if (pidProfiles(i)->vbat_sag_compensation > 0) {
sagCompensationConfigured = true;
}
}
#endif
}
//
// ESC
//
#ifdef USE_ESC_SENSOR
typedef struct voltageMeterESCState_s {
uint16_t voltageFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageDisplayFiltered; // battery voltage in 0.01V steps (filtered)
uint16_t voltageUnfiltered; // battery voltage in 0.01V steps (unfiltered)
pt1Filter_t filter;
pt1Filter_t displayFilter;
} voltageMeterESCState_t;
static voltageMeterESCState_t voltageMeterESCState;
@ -218,7 +262,7 @@ void voltageMeterESCInit(void)
{
#ifdef USE_ESC_SENSOR
memset(&voltageMeterESCState, 0, sizeof(voltageMeterESCState_t));
pt1FilterInit(&voltageMeterESCState.filter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatLpfPeriod), HZ_TO_INTERVAL(50)));
pt1FilterInit(&voltageMeterESCState.displayFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatDisplayLpfPeriod), HZ_TO_INTERVAL(isSagCompensationConfigured() ? FAST_VOLTAGE_TASK_FREQ_HZ : SLOW_VOLTAGE_TASK_FREQ_HZ)));
#endif
}
@ -228,7 +272,7 @@ void voltageMeterESCRefresh(void)
escSensorData_t *escData = getEscSensorData(ESC_SENSOR_COMBINED);
if (escData) {
voltageMeterESCState.voltageUnfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
voltageMeterESCState.voltageFiltered = pt1FilterApply(&voltageMeterESCState.filter, voltageMeterESCState.voltageUnfiltered);
voltageMeterESCState.voltageDisplayFiltered = pt1FilterApply(&voltageMeterESCState.displayFilter, voltageMeterESCState.voltageUnfiltered);
}
#endif
}
@ -242,7 +286,7 @@ void voltageMeterESCReadMotor(uint8_t motorNumber, voltageMeter_t *voltageMeter)
escSensorData_t *escData = getEscSensorData(motorNumber);
if (escData) {
voltageMeter->unfiltered = escData->dataAge <= ESC_BATTERY_AGE_MAX ? escData->voltage : 0;
voltageMeter->filtered = voltageMeter->unfiltered; // no filtering for ESC motors currently.
voltageMeter->displayFiltered = voltageMeter->unfiltered; // no filtering for ESC motors currently.
} else {
voltageMeterReset(voltageMeter);
}
@ -255,7 +299,7 @@ void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter)
#ifndef USE_ESC_SENSOR
voltageMeterReset(voltageMeter);
#else
voltageMeter->filtered = voltageMeterESCState.voltageFiltered;
voltageMeter->displayFiltered = voltageMeterESCState.voltageDisplayFiltered;
voltageMeter->unfiltered = voltageMeterESCState.voltageUnfiltered;
#endif
}

12
src/main/sensors/voltage.h
View file

@ -22,6 +22,9 @@
#include "voltage_ids.h"
#define SLOW_VOLTAGE_TASK_FREQ_HZ 50
#define FAST_VOLTAGE_TASK_FREQ_HZ 200
//
// meters
//
@ -38,8 +41,11 @@ extern const char * const voltageMeterSourceNames[VOLTAGE_METER_COUNT];
// WARNING - do not mix usage of VOLTAGE_METER_* and VOLTAGE_SENSOR_*, they are separate concerns.
typedef struct voltageMeter_s {
uint16_t filtered; // voltage in 0.01V steps
uint16_t displayFiltered; // voltage in 0.01V steps
uint16_t unfiltered; // voltage in 0.01V steps
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
uint16_t sagFiltered; // voltage in 0.01V steps
#endif
bool lowVoltageCutoff;
} voltageMeter_t;
@ -94,6 +100,8 @@ PG_DECLARE_ARRAY(voltageSensorADCConfig_t, MAX_VOLTAGE_SENSOR_ADC, voltageSensor
//
void voltageMeterReset(voltageMeter_t *voltageMeter);
void voltageMeterGenericInit(void);
void voltageMeterADCInit(void);
void voltageMeterADCRefresh(void);
void voltageMeterADCRead(voltageSensorADC_e adcChannel, voltageMeter_t *voltageMeter);
@ -112,3 +120,5 @@ extern const uint8_t voltageMeterADCtoIDMap[MAX_VOLTAGE_SENSOR_ADC];
extern const uint8_t supportedVoltageMeterCount;
extern const uint8_t voltageMeterIds[];
void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *voltageMeter);
bool isSagCompensationConfigured(void);

1
src/main/target/common_pre.h
View file

@ -367,4 +367,5 @@
#define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol
#define USE_INTERPOLATED_SP
#define USE_CUSTOM_BOX_NAMES
#define USE_BATTERY_VOLTAGE_SAG_COMPENSATION
#endif