From b3f95b40f09f212dfa2775888718d79aa5653c1f Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Sat, 6 Jul 2024 16:59:08 +0200
Subject: [PATCH] vbat_auto_vdrop: refactor

---
 src/io/vbat.c | 58 +++++++++++++++++++++++----------------------------
 1 file changed, 26 insertions(+), 32 deletions(-)

diff --git a/src/io/vbat.c b/src/io/vbat.c
index 57e4e8714..f751a18fc 100644
--- a/src/io/vbat.c
+++ b/src/io/vbat.c
@@ -40,41 +40,35 @@ void vbat_init() {
 }
 
 static float vbat_auto_vdrop(float thrfilt, float tempvolt) {
-  static float lastout[12];
-  static float lastin[12];
-  static float vcomp[12];
-  static float score[12];
-  static int z = 0;
   static int minindex = 0;
-  static int firstrun = 1;
 
-  if (thrfilt > 0.1f) {
-    vcomp[z] = tempvolt + (float)z * 0.1f * thrfilt;
+  if (thrfilt <= 0.1f) {
+    return minindex * 0.1f;
+  }
 
-    if (firstrun) {
-      for (int y = 0; y < 12; y++)
-        lastin[y] = vcomp[z];
-      firstrun = 0;
-    }
-    //  y(n) = x(n) - x(n-1) + R * y(n-1)
-    //  out = in - lastin + coeff*lastout
-    // hpf
-    const float ans = vcomp[z] - lastin[z] + lpfcalc(1000 * 12, 6000e3) * lastout[z];
-    lastin[z] = vcomp[z];
-    lastout[z] = ans;
-    lpf(&score[z], ans * ans, lpfcalc(1000 * 12, 60e6));
-    z++;
-
-    if (z >= 12) {
-      z = 0;
-      float min = score[0];
-      for (int i = 0; i < 12; i++) {
-        if ((score[i]) < min) {
-          min = (score[i]);
-          minindex = i;
-          // add an offset because it seems to be usually early
-          minindex++;
-        }
+  static int z = 0;
+  static float lastin[12];
+  static float lastout[12];
+
+  //  y(n) = x(n) - x(n-1) + R * y(n-1)
+  //  out = in - lastin + coeff*lastout
+  const float vcomp = tempvolt + (float)z * 0.1f * thrfilt;
+  const float ans = vcomp - lastin[z] + lpfcalc(1000 * 12, 6000e3) * lastout[z];
+  lastin[z] = vcomp;
+  lastout[z] = ans;
+
+  static float score[12];
+  lpf(&score[z], ans * ans, lpfcalc(1000 * 12, 60e6));
+  z++;
+
+  if (z >= 12) {
+    z = 0;
+    float min = score[0];
+    for (int i = 0; i < 12; i++) {
+      if ((score[i]) < min) {
+        min = (score[i]);
+        // add an offset because it seems to be usually early
+        minindex = i + 1;
       }
     }
   }