Moving battery calculation for lipo discharge into utility.c

src/devices/logitech_gpro.c

@@ -1,12 +1,13 @@
-#include "../device.h"
-#include "../utility.h"
-#include "logitech.h"
-
 #include <unistd.h>
 
 #include <math.h>
+#include <stdint.h>
 #include <string.h>
 
+#include "../device.h"
+#include "../utility.h"
+#include "logitech.h"
+
 static struct device device_gpro;
 
 #define ID_LOGITECH_PRO     0x0aa7
@@ -19,6 +20,16 @@ static const uint16_t PRODUCT_IDS[] = {
     ID_LOGITECH_PRO_X_1,
 };
 
+static const double battery_estimate_terms[] = {
+    -1.7790085824253613e+006,
+    2.3692153307344888e+003,
+    -1.2580905041506840e+000,
+    3.3295201187388395e-004,
+    -4.3913981733597497e-008,
+    2.3093121045539907e-012
+};
+static const size_t num_terms = 6;
+
 static int gpro_send_sidetone(hid_device* device_handle, uint8_t num);
 static int gpro_request_battery(hid_device* device_handle);
 static int gpro_send_inactive_time(hid_device* device_handle, uint8_t num);
@@ -52,44 +63,6 @@ static int gpro_send_sidetone(hid_device* device_handle, uint8_t num)
     return hid_write(device_handle, sidetone_data, sizeof(sidetone_data) / sizeof(sidetone_data[0]));
 }
 
-/**
- * @brief This function calculates the estimate batttery level in percent.
- *
- * Battery stats:
- *   - Voltage capacity:  4200 (100%)
- *   - Charging voltage:  4700
- *   - Power off voltage: 3300 (  0%)
- *
- * @param voltage readings
- * @return battery level in percent
- */
-static float estimate_battery_level(uint16_t voltage)
-{
-    double terms[] = {
-        -1.7790085824253613e+006,
-        2.3692153307344888e+003,
-        -1.2580905041506840e+000,
-        3.3295201187388395e-004,
-        -4.3913981733597497e-008,
-        2.3093121045539907e-012
-    };
-
-    size_t csz = sizeof terms / sizeof *terms;
-
-    double t       = 1;
-    double percent = 0;
-    for (int i = 0; i < csz; i++) {
-        percent += terms[i] * t;
-        t *= voltage;
-    }
-
-    if (percent > 100)
-        percent = 100;
-    if (percent < 0)
-        percent = 0;
-    return percent;
-}
-
 static int gpro_request_battery(hid_device* device_handle)
 {
     /*
@@ -121,7 +94,7 @@ static int gpro_request_battery(hid_device* device_handle)
         return HSC_ERROR;
 
     const uint16_t voltage = (buf[4] << 8) | buf[5];
-    return (int)(roundf(estimate_battery_level(voltage)));
+    return (int)(roundf(poly_battery_level(battery_estimate_terms, num_terms, voltage)));
 }
 
 static int gpro_send_inactive_time(hid_device* device_handle, uint8_t num)

src/utility.c

@@ -1,6 +1,25 @@
+#include <stdint.h>
+#include <unistd.h>
+
 #include "utility.h"
 
 int map(int x, int in_min, int in_max, int out_min, int out_max)
 {
     return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
 }
+
+float poly_battery_level(const double terms[], const size_t numterms, uint16_t voltage)
+{
+    double t       = 1;
+    double percent = 0;
+    for (int i = 0; i < numterms; i++) {
+        percent += terms[i] * t;
+        t *= voltage;
+    }
+
+    if (percent > 100)
+        percent = 100;
+    if (percent < 0)
+        percent = 0;
+    return percent;
+}

src/utility.h

@@ -8,3 +8,16 @@
  *  @return the mapped value
  */
 int map(int x, int in_min, int in_max, int out_min, int out_max);
+
+/**
+ * @brief This function calculates the estimate batttery level in percent.
+ *
+ * To find the terms representing the polynominal discarge curve of the
+ * battery an solver like https://arachnoid.com/polysolve/ can be used.
+ *
+ * @param array polynominal terms for the battery discharge curve
+ * @param number of terms
+ * @param voltage readings
+ * @return battery level in percent
+ */
+float poly_battery_level(const double terms[], const size_t numterms, uint16_t voltage);