battery - fix BATTERY_NOT_PRESENT detection, detection logic change (#…

…13599)

* battery - fix BATTERY_NOT_PRESENT detection, detection logic change

Detection logic is refactored - battery voltage delta is tested each
50ms, voltage is considered stable when difference of last 10 samples
is smaller than 100mV
This makes stable threshold more forgiving than #13350 (time for
comparison is 50 instead of 500ms).

* battery - improve stable voltage detection

- voltageStablePrevFiltered every time delta is exceeded
- voltage within range is ANDed over 100ms periods
- voltage is stable if it was within range for 10 out of 11 periods
  - slowly changing voltage will update threshold, but voltage will be
    considered stable
  - 1 update/s (100mV/s) is tolerated

* battery - fuix typos, improve comments

src/main/sensors/battery.c

@@ -67,7 +67,6 @@
  *
  */
 
-#define VBAT_STABLE_MAX_DELTA 10 // 100mV
 #define LVC_AFFECT_TIME 10000000 //10 secs for the LVC to slowly kick in
 
 // Battery monitoring stuff
@@ -159,20 +158,14 @@ void batteryUpdateVoltage(timeUs_t currentTimeUs)
             break;
     }
 
-    if (!voltageMeter.isVoltageStable && cmp32(millis(), voltageMeter.prevDisplayFilteredTime) >= PREV_DISPLAY_FILTERED_TIME_DIFF) {
-        if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBattery()) {
-            voltageMeter.isVoltageStable = abs(voltageMeter.prevDisplayFiltered - voltageMeter.displayFiltered) <= VBAT_STABLE_MAX_DELTA;
-        }
-
-        voltageMeter.prevDisplayFiltered = voltageMeter.displayFiltered;
-        voltageMeter.prevDisplayFilteredTime = millis();
-    }
+    voltageStableUpdate(&voltageMeter);
 
     DEBUG_SET(DEBUG_BATTERY, 0, voltageMeter.unfiltered);
     DEBUG_SET(DEBUG_BATTERY, 1, voltageMeter.displayFiltered);
-    DEBUG_SET(DEBUG_BATTERY, 4, voltageMeter.isVoltageStable ? 1 : 0);
+    DEBUG_SET(DEBUG_BATTERY, 3, voltageMeter.voltageStableBits);
+    DEBUG_SET(DEBUG_BATTERY, 4, voltageIsStable(&voltageMeter) ? 1 : 0);
     DEBUG_SET(DEBUG_BATTERY, 5, isVoltageFromBattery() ? 1 : 0);
-    DEBUG_SET(DEBUG_BATTERY, 6, voltageMeter.prevDisplayFiltered);
+    DEBUG_SET(DEBUG_BATTERY, 6, voltageMeter.voltageStablePrevFiltered);
     DEBUG_SET(DEBUG_BATTERY, 7, voltageState);
 }
 
@@ -205,7 +198,9 @@ bool isVoltageFromBattery(void)
 
 void batteryUpdatePresence(void)
 {
-    if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT) && isVoltageFromBattery() && voltageMeter.isVoltageStable) {
+    if ((voltageState == BATTERY_NOT_PRESENT || voltageState == BATTERY_INIT)
+        && isVoltageFromBattery()
+        && voltageIsStable(&voltageMeter)) {
         // Battery has just been connected - calculate cells, warning voltages and reset state
 
         consumptionState = voltageState = BATTERY_OK;
@@ -232,11 +227,12 @@ void batteryUpdatePresence(void)
         batteryCriticalHysteresisVoltage = (batteryCriticalVoltage > batteryConfig()->vbathysteresis) ? batteryCriticalVoltage - batteryConfig()->vbathysteresis : 0;
         lowVoltageCutoff.percentage = 100;
         lowVoltageCutoff.startTime = 0;
-    } else if (voltageState != BATTERY_NOT_PRESENT && voltageMeter.isVoltageStable && !isVoltageFromBattery()) {
+    } else if (voltageState != BATTERY_NOT_PRESENT
+               && voltageIsStable(&voltageMeter)
+               && !isVoltageFromBattery()) {
         /* battery has been disconnected - can take a while for filter cap to disharge so we use a threshold of batteryConfig()->vbatnotpresentcellvoltage */
         consumptionState = voltageState = BATTERY_NOT_PRESENT;
-
-        voltageMeter.isVoltageStable = false;
+
         batteryCellCount = 0;
         batteryWarningVoltage = 0;
         batteryCriticalVoltage = 0;

src/main/sensors/voltage.c

@@ -21,6 +21,7 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
+#include <stdlib.h>
 
 #include "platform.h"
 
@@ -89,10 +90,10 @@ const uint8_t supportedVoltageMeterCount = ARRAYLEN(voltageMeterIds);
 void voltageMeterReset(voltageMeter_t *meter)
 {
     meter->displayFiltered = 0;
-    meter->prevDisplayFiltered = 0;
-    meter->prevDisplayFilteredTime = 0;
+    meter->voltageStablePrevFiltered = 0;
+    meter->voltageStableLastUpdate = 0;
+    meter->voltageStableBits = 0;
     meter->unfiltered = 0;
-    meter->isVoltageStable = false;
 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
     meter->sagFiltered = 0;
 #endif
@@ -362,3 +363,28 @@ void voltageMeterRead(voltageMeterId_e id, voltageMeter_t *meter)
         voltageMeterReset(meter);
     }
 }
+
+// update voltageStableBits
+// new 1 bit (= stable) is shifted in every VOLTAGE_STABLE_UPDATE_MS
+// when difference is larger than VOLTAGE_STABLE_MAX_DELTA, LSB of shift register is
+//   reset to 0 (logical AND) and voltageStablePrevFiltered is updated with new voltage value
+void voltageStableUpdate(voltageMeter_t* vm)
+{
+    const uint32_t now = millis();
+    // test voltage on each call
+    if (abs(vm->voltageStablePrevFiltered - vm->displayFiltered) > VOLTAGE_STABLE_MAX_DELTA) {
+        // reset stable voltage reference
+        vm->voltageStablePrevFiltered = vm->displayFiltered;
+        vm->voltageStableBits &= ~BIT(0);  // voltage threshold exceeded in this period, clear LSB/BIT(0)
+    }
+    if (cmp32(now, vm->voltageStableLastUpdate) >= VOLTAGE_STABLE_TICK_MS) {
+        vm->voltageStableBits = (vm->voltageStableBits << 1) | BIT(0);  // start with 'stable' state
+        vm->voltageStableLastUpdate = now;
+    }
+}
+
+// voltage is stable when it was within VOLTAGE_STABLE_MAX_DELTA for 10 update periods
+bool voltageIsStable(voltageMeter_t* vm)
+{
+    return __builtin_popcount(vm->voltageStableBits & (BIT(VOLTAGE_STABLE_BITS_TOTAL + 1) - 1)) >= VOLTAGE_STABLE_BITS_THRESHOLD;
+}

src/main/sensors/voltage.h

@@ -25,7 +25,12 @@
 #define SLOW_VOLTAGE_TASK_FREQ_HZ 50
 #define FAST_VOLTAGE_TASK_FREQ_HZ 200
 
-#define PREV_DISPLAY_FILTERED_TIME_DIFF 500 // ms
+#define VOLTAGE_STABLE_TICK_MS        100
+#define VOLTAGE_STABLE_BITS_TOTAL     11    // number of samples to test
+#define VOLTAGE_STABLE_BITS_THRESHOLD 10    // number of samples tham must be within delta (~1s)
+#define VOLTAGE_STABLE_MAX_DELTA      10    // 100mV
+
+STATIC_ASSERT(VOLTAGE_STABLE_BITS_TOTAL < sizeof(uint16_t) * 8, "not enough voltageStableBits");
 
 //
 // meters
@@ -43,11 +48,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 displayFiltered;               // voltage in 0.01V steps
-    uint16_t prevDisplayFiltered;               // voltage in 0.01V steps
-    timeMs_t prevDisplayFilteredTime;
-    bool isVoltageStable;
-    uint16_t unfiltered;                    // voltage in 0.01V steps
+    uint16_t displayFiltered;                // voltage in 0.01V steps
+    uint16_t voltageStablePrevFiltered;      // last filtered voltage sample
+    timeMs_t voltageStableLastUpdate;
+    uint16_t voltageStableBits;              // rolling bitmask, bit 1 if battery difference is within threshold, shifted left
+    uint16_t unfiltered;                     // voltage in 0.01V steps
 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
     uint16_t sagFiltered;                   // voltage in 0.01V steps
 #endif
@@ -116,6 +121,12 @@ void voltageMeterESCRefresh(void);
 void voltageMeterESCReadCombined(voltageMeter_t *voltageMeter);
 void voltageMeterESCReadMotor(uint8_t motor, voltageMeter_t *voltageMeter);
 
+//
+// api for battery stable voltage detection
+//
+
+void voltageStableUpdate(voltageMeter_t* vm);
+bool voltageIsStable(voltageMeter_t* vm);
 
 //
 // API for reading/configuring current meters by id.