Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-20 23:05:19 +03:00
Code Issues Wiki Activity

motor output scale

Browse source 
First draft

Change method to percentage compensation

fast sag filter with fast battery updates

Renaming, moving factors to init where possible

Names changed, display update frequency reverted to 50hz as it was

50Hz ESC Voltage sampling, battery sag lowpass for PID compensation.

increment PG_PID_PROFILE, element added to end of batteryConfig_t

all HZ_TO_INTERVALs set back to 200 to match battery task frequency of 200hz.

Add a flag to control vbat comp

Flag vbat_sag_comp_enabled allows battery compensation to be enabled or
disabled from the CLI. When disabled the battery voltage task is run at
50Hz and the battery compensation code is not run. When enabled the
voltage task is run at 200Hz and the compensation code runs. Constants
for the fast and slow rates are added to tasks.h. The default value for
vbat_sag_compensation is changed to 100 as we no longer need to use it
to disable the feature.

Fixed variable task frequency setting.

Added config validation to disable sag compensation unless ADC is used as the voltage data source.

Added conditionals, fixed naming.

Fixed build.
This commit is contained in:
ctzsnooze 2020-03-08 00:16:02 +11:00 • committed by mikeller
parent 9c3d4603b7
commit d63ba914c6
12 changed files with 158 additions and 50 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 = { 0, 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) },
@ -1006,6 +1009,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) {

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

@ -229,6 +229,12 @@ 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
rescheduleTask(TASK_BATTERY_VOLTAGE, TASK_PERIOD_HZ(getBatteryVoltageTaskFrequencyHz()));
#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 +384,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

37
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,17 @@ 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) {
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 +638,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;
}

50
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();
}
@ -465,7 +462,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 +476,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 +498,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 +518,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);

69
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,9 @@ const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds);
void voltageMeterReset(voltageMeter_t *meter)
{
meter->filtered = 0;
meter->displayFiltered = 0;
meter->unfiltered = 0;
meter->sagFiltered = 0;
}
//
// ADC
@ -104,9 +108,13 @@ 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];
@ -163,17 +171,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 (currentPidProfile->vbat_sag_compensation > 0) {
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 +199,25 @@ 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
}
uint16_t getBatteryVoltageTaskFrequencyHz(void)
{
uint16_t frequencyHz = SLOW_VOLTAGE_TASK_FREQ_HZ;
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
for (unsigned i = 0; i < PID_PROFILE_COUNT; i++) {
if (pidProfiles(i)->vbat_sag_compensation > 0) {
frequencyHz = FAST_VOLTAGE_TASK_FREQ_HZ;
}
}
#endif
return frequencyHz;
}
void voltageMeterADCInit(void)
@ -194,7 +228,12 @@ 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(getBatteryVoltageTaskFrequencyHz())));
#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
if (currentPidProfile->vbat_sag_compensation > 0) {
pt1FilterInit(&state->sagFilter, pt1FilterGain(GET_BATTERY_LPF_FREQUENCY(batteryConfig()->vbatSagLpfPeriod), HZ_TO_INTERVAL(getBatteryVoltageTaskFrequencyHz())));
}
#endif
}
}
@ -204,9 +243,9 @@ void voltageMeterADCInit(void)
#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 +257,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(getBatteryVoltageTaskFrequencyHz())));
#endif
}
@ -228,7 +267,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 +281,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 +294,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
}

10
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;
@ -112,3 +118,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);
uint16_t getBatteryVoltageTaskFrequencyHz(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