Merge pull request #3510 from brycedjohnson/LVC

Add in Low Voltage Cutout option/Increase Vbat filtering/Improved cell detection

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

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();
 }
 

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) },

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 - voltageMeter.unfiltered) <= VBAT_STABLE_MAX_DELTA;
-
-    bool isVoltageStable = ABS(voltageMeter.filtered - previousVoltage) <= 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[2] = batteryCellCount;
-        debug[3] = 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) {

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

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)
 {

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.