diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index bccd2f6265..9f5447667a 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -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) }, diff --git a/src/main/config/config.c b/src/main/config/config.c index d0c9cbdee8..83dcc7ce92 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -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) { diff --git a/src/main/fc/tasks.c b/src/main/fc/tasks.c index 279e1138cd..8cb6fd29f3 100644 --- a/src/main/fc/tasks.c +++ b/src/main/fc/tasks.c @@ -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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 067a55414c..25099c9d63 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -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; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index dd48167cfb..dd62bf0465 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 309c046df8..ec6b41467a 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/msp/msp.c b/src/main/msp/msp.c index 6638a70e09..17f407c9cc 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -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; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 7764bcf391..d82bce6f74 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -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; diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index a627ee6b96..943a85621c 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -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); diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index f9a0de560b..ef47669a81 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -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 } diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 11566cfb09..6a66c37b52 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -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); diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 59c2ee2828..5fcfaf04dc 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -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