diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 1957d3a542..b620aefd16 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -44,6 +44,8 @@ #include "scheduler/scheduler.h" +#include "sensors/battery.h" + #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/pid.h" @@ -299,9 +301,15 @@ void updateRcCommands(void) if (feature(FEATURE_3D)) { tmp = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - PWM_RANGE_MIN); + if (getLowVoltageCutoff()->enabled) { + tmp = tmp * getLowVoltageCutoff()->percentage / 100; + } } else { tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->mincheck); + if (getLowVoltageCutoff()->enabled) { + tmp = tmp * getLowVoltageCutoff()->percentage / 100; + } } rcCommand[THROTTLE] = rcLookupThrottle(tmp); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 9ddfc92d9b..2e21e54549 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -131,14 +131,11 @@ static void taskHandleSerial(timeUs_t currentTimeUs) void taskBatteryAlerts(timeUs_t currentTimeUs) { - UNUSED(currentTimeUs); - if (!ARMING_FLAG(ARMED)) { // the battery *might* fall out in flight, but if that happens the FC will likely be off too unless the user has battery backup. batteryUpdatePresence(); } - batteryUpdateStates(); - + batteryUpdateStates(currentTimeUs); batteryUpdateAlarms(); } diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index ac722557a3..9ee2464064 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -435,6 +435,7 @@ const clivalue_t valueTable[] = { { "use_vbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useVBatAlerts) }, { "use_cbat_alerts", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, useConsumptionAlerts) }, { "cbat_alert_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, consumptionWarningPercentage) }, + { "vbat_cutoff_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_BATTERY_CONFIG, offsetof(batteryConfig_t, lvcPercentage) }, // PG_VOLTAGE_SENSOR_ADC_CONFIG { "vbat_scale", VAR_UINT8 | MASTER_VALUE, .config.minmax = { VBAT_SCALE_MIN, VBAT_SCALE_MAX }, PG_VOLTAGE_SENSOR_ADC_CONFIG, offsetof(voltageSensorADCConfig_t, vbatscale) }, diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e7d774ade8..6236aaf126 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -56,11 +56,13 @@ static void batteryUpdateConsumptionState(void); // temporary forward reference #define VBAT_STABLE_MAX_DELTA 2 +#define LVC_AFFECT_TIME 10000000 //10 secs for the LVC to slowly kick in // Battery monitoring stuff uint8_t batteryCellCount; // Note: this can be 0 when no battery is detected or when the battery voltage sensor is missing or disabled. uint16_t batteryWarningVoltage; uint16_t batteryCriticalVoltage; +static lowVoltageCutoff_t lowVoltageCutoff; // static currentMeter_t currentMeter; static voltageMeter_t voltageMeter; @@ -94,6 +96,7 @@ PG_RESET_TEMPLATE(batteryConfig_t, batteryConfig, .vbatwarningcellvoltage = 35, .vbatnotpresentcellvoltage = 30, //A cell below 3 will be ignored .voltageMeterSource = DEFAULT_VOLTAGE_METER_SOURCE, + .lvcPercentage = 100, //Off by default at 100% // current .batteryCapacity = 0, @@ -121,7 +124,7 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs) #endif case VOLTAGE_METER_ADC: voltageMeterADCRefresh(); - voltageMeterADCRead(ADC_BATTERY, &voltageMeter); + voltageMeterADCRead(VOLTAGE_SENSOR_ADC_VBAT, &voltageMeter); break; default: @@ -155,9 +158,7 @@ static void updateBatteryBeeperAlert(void) void batteryUpdatePresence(void) { - static uint16_t previousVoltage = 0; - - bool isVoltageStable = ABS(voltageMeter.filtered - previousVoltage) <= VBAT_STABLE_MAX_DELTA; + bool isVoltageStable = ABS(voltageMeter.filtered - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA; bool isVoltageFromBat = (voltageMeter.filtered >= batteryConfig()->vbatnotpresentcellvoltage //above ~0V && voltageMeter.filtered <= batteryConfig()->vbatmaxcellvoltage) //1s max cell voltage check @@ -183,6 +184,8 @@ void batteryUpdatePresence(void) batteryCellCount = cells; batteryWarningVoltage = batteryCellCount * batteryConfig()->vbatwarningcellvoltage; batteryCriticalVoltage = batteryCellCount * batteryConfig()->vbatmincellvoltage; + lowVoltageCutoff.percentage = 100; + lowVoltageCutoff.startTime = 0; } else if ( voltageState != BATTERY_NOT_PRESENT && isVoltageStable @@ -196,13 +199,10 @@ void batteryUpdatePresence(void) batteryWarningVoltage = 0; batteryCriticalVoltage = 0; } - if (debugMode == DEBUG_BATTERY) { - debug[2] = voltageState; - debug[3] = batteryCellCount; + debug[2] = batteryCellCount; + debug[3] = isVoltageStable; } - - previousVoltage = voltageMeter.filtered; // record the current value so we can detect voltage stabilisation next the presence needs updating. } static void batteryUpdateVoltageState(void) @@ -232,16 +232,42 @@ static void batteryUpdateVoltageState(void) default: break; } + } -void batteryUpdateStates(void) +static void batteryUpdateLVC(timeUs_t currentTimeUs) +{ + if (batteryConfig()->lvcPercentage < 100) { + if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) { + lowVoltageCutoff.enabled = true; + lowVoltageCutoff.startTime = currentTimeUs; + lowVoltageCutoff.percentage = 100; + } + if (lowVoltageCutoff.enabled) { + if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) { + lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME); + } + else { + lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage; + } + } + } + +} + +void batteryUpdateStates(timeUs_t currentTimeUs) { batteryUpdateVoltageState(); batteryUpdateConsumptionState(); - + batteryUpdateLVC(currentTimeUs); batteryState = MAX(voltageState, consumptionState); } +lowVoltageCutoff_t *getLowVoltageCutoff(void) +{ + return &lowVoltageCutoff; +} + batteryState_e getBatteryState(void) { return batteryState; @@ -268,6 +294,9 @@ void batteryInit(void) voltageState = BATTERY_NOT_PRESENT; batteryWarningVoltage = 0; batteryCriticalVoltage = 0; + lowVoltageCutoff.enabled = false; + lowVoltageCutoff.percentage = 100; + lowVoltageCutoff.startTime = 0; voltageMeterReset(&voltageMeter); switch (batteryConfig()->voltageMeterSource) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 6695a7e1f0..f04e5f1543 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -30,7 +30,7 @@ typedef struct batteryConfig_s { uint8_t vbatmincellvoltage; // minimum voltage per cell, this triggers battery critical alarm, in 0.1V units, default is 33 (3.3V) uint8_t vbatwarningcellvoltage; // warning voltage per cell, this triggers battery warning alarm, in 0.1V units, default is 35 (3.5V) uint8_t vbatnotpresentcellvoltage; // Between vbatmaxcellvoltage and 2*this is considered to be USB powered. Below this it is notpresent - + uint8_t lvcPercentage; // Percentage of throttle when lvc is triggered voltageMeterSource_e voltageMeterSource; // source of battery voltage meter used, either ADC or ESC // current @@ -42,8 +42,15 @@ typedef struct batteryConfig_s { bool useConsumptionAlerts; // Issue alerts based on total power consumption uint8_t consumptionWarningPercentage; // Percentage of remaining capacity that should trigger a battery warning uint8_t vbathysteresis; // hysteresis for alarm, default 1 = 0.1V + } batteryConfig_t; +typedef struct lowVoltageCutoff_s { + bool enabled; + uint8_t percentage; + timeUs_t startTime; +} lowVoltageCutoff_t; + PG_DECLARE(batteryConfig_t, batteryConfig); typedef enum { @@ -60,7 +67,7 @@ void batteryUpdatePresence(void); batteryState_e getBatteryState(void); const char * getBatteryStateString(void); -void batteryUpdateStates(void); +void batteryUpdateStates(timeUs_t currentTimeUs); void batteryUpdateAlarms(void); struct rxConfig_s; @@ -76,3 +83,5 @@ int32_t getAmperageLatest(void); int32_t getMAhDrawn(void); void batteryUpdateCurrentMeter(timeUs_t currentTimeUs); + +lowVoltageCutoff_t *getLowVoltageCutoff(void); diff --git a/src/main/sensors/voltage.c b/src/main/sensors/voltage.c index 3369edc042..fa0430cd54 100644 --- a/src/main/sensors/voltage.c +++ b/src/main/sensors/voltage.c @@ -176,7 +176,7 @@ void voltageMeterADCInit(void) voltageMeterADCState_t *state = &voltageMeterADCStates[i]; memset(state, 0, sizeof(voltageMeterADCState_t)); - biquadFilterInitLPF(&state->filter, VBATT_LPF_FREQ, 50000); + biquadFilterInitLPF(&state->filter, VBAT_LPF_FREQ, 50000); } } @@ -194,7 +194,7 @@ typedef struct voltageMeterESCState_s { static voltageMeterESCState_t voltageMeterESCState; #endif -#define VBAT_LPF_FREQ 0.4f + void voltageMeterESCInit(void) { diff --git a/src/main/sensors/voltage.h b/src/main/sensors/voltage.h index 0c8d3d6823..e1566fdffa 100644 --- a/src/main/sensors/voltage.h +++ b/src/main/sensors/voltage.h @@ -34,6 +34,7 @@ typedef enum { typedef struct voltageMeter_s { uint16_t filtered; // voltage in 0.1V steps uint16_t unfiltered; // voltage in 0.1V steps + bool lowVoltageCutoff; } voltageMeter_t; @@ -54,7 +55,7 @@ typedef enum { #define VBAT_SCALE_MIN 0 #define VBAT_SCALE_MAX 255 -#define VBATT_LPF_FREQ 1.0f +#define VBAT_LPF_FREQ 0.1f #ifndef MAX_VOLTAGE_SENSOR_ADC #define MAX_VOLTAGE_SENSOR_ADC 1 // VBAT - some boards have external, 12V, 9V and 5V meters.