From c8f40160bb0091e68fb8493ff64b07573fa23823 Mon Sep 17 00:00:00 2001
From: Tobias Schramm <t.schramm@t-sys.eu>
Date: Tue, 9 Feb 2021 13:48:33 +0100
Subject: [PATCH] Implement SOC estimation

---
 src/battery.c                   | 42 +++++++++++++++++++++++++++++++++
 src/battery.h                   |  3 ++-
 src/main.c                      | 12 ++++++----
 zephyr/pinetime_devkit0.overlay |  7 ++++++
 4 files changed, 59 insertions(+), 5 deletions(-)

diff --git a/src/battery.c b/src/battery.c
index 05ef76c..cb491ec 100644
--- a/src/battery.c
+++ b/src/battery.c
@@ -5,6 +5,7 @@
 
 #define VBATT DT_PATH(vbatt)
 #define BATTERY_ADC_GAIN ADC_GAIN_1_6
+#define BATTERY DT_PATH(battery)
 
 static struct {
     const struct device *adc;
@@ -65,4 +66,45 @@ int battery_update() {
 
 uint16_t battery_get_voltage_mv() {
     return battery_adc_config.value_mv;
+}
+
+const uint32_t battery_soc_voltage_mv_pairs[] = DT_PROP(BATTERY, soc_voltage_mv);
+
+#if DT_PROP_LEN(BATTERY, soc_voltage_mv) % 2 != 0
+#error length of soc_voltage_mv must be divisible by two!
+#endif
+
+uint32_t battery_voltage_mv_to_soc(uint16_t batt_mv) {
+    int i = 0;
+    uint32_t last_soc;
+    uint32_t last_voltage_mv;
+
+    if (!ARRAY_SIZE(battery_soc_voltage_mv_pairs)) {
+        return 100;
+    }
+
+    last_soc = battery_soc_voltage_mv_pairs[0];
+    last_voltage_mv = battery_soc_voltage_mv_pairs[1];
+
+    if (batt_mv >= last_voltage_mv) {
+        return last_soc;
+    }
+
+    for (i = 1; i < ARRAY_SIZE(battery_soc_voltage_mv_pairs) / 2; i++) {
+        uint32_t soc = battery_soc_voltage_mv_pairs[i * 2];
+        uint32_t voltage_mv = battery_soc_voltage_mv_pairs[i * 2 + 1];
+
+        if (batt_mv >= voltage_mv && batt_mv <= last_voltage_mv) {
+            uint32_t delta_mv = last_voltage_mv - voltage_mv;
+            uint32_t delta_soc = last_soc - soc;
+            uint32_t frac_mv = batt_mv - voltage_mv;
+
+            return soc + frac_mv * delta_soc / delta_mv;
+        }
+
+        last_soc = soc;
+        last_voltage_mv = voltage_mv;
+    }
+
+    return last_soc;
 }
\ No newline at end of file
diff --git a/src/battery.h b/src/battery.h
index 55012f4..be66fc6 100644
--- a/src/battery.h
+++ b/src/battery.h
@@ -4,4 +4,5 @@
 
 int battery_init();
 int battery_update();
-uint16_t battery_get_voltage_mv();
\ No newline at end of file
+uint16_t battery_get_voltage_mv();
+uint32_t battery_voltage_mv_to_soc(uint16_t batt_mv);
\ No newline at end of file
diff --git a/src/main.c b/src/main.c
index 69871bc..3ae76f9 100644
--- a/src/main.c
+++ b/src/main.c
@@ -6,7 +6,7 @@
  * SPDX-License-Identifier: Apache-2.0
  */
 
-
+#include <stdio.h>
 #include <sys/printk.h>
 #include <bluetooth/hci.h>
 #include <random/rand32.h>
@@ -49,6 +49,7 @@ void main(void)
 	}
 
 	platform_display_draw_string(0, DISPLAY_LINE_BATTERY_VOLTAGE, "Battery voltage:");
+	platform_display_draw_string(0, DISPLAY_LINE_BATTERY_VOLTAGE + 2, "Battery SOC:");
 
     // first init everything
 	// Use custom randomization as the mbdet_tls context initialization messes with the Zeyhr BLE stack.
@@ -92,10 +93,13 @@ void main(void)
 		if (err) {
 			printk("Failed to update battery voltage: %d\n", err);
 		} else {
-			char voltage_mv[12] = { 0 };
+			char tmpstr[12] = { 0 };
 
-			snprintf(voltage_mv, sizeof(voltage_mv), "%04u mV", battery_get_voltage_mv());
-			platform_display_draw_string(0, DISPLAY_LINE_BATTERY_VOLTAGE + 1, voltage_mv);
+			snprintf(tmpstr, sizeof(tmpstr), "%04u mV", battery_get_voltage_mv());
+			platform_display_draw_string(0, DISPLAY_LINE_BATTERY_VOLTAGE + 1, tmpstr);
+
+			snprintf(tmpstr, sizeof(tmpstr), "%u%%", battery_voltage_mv_to_soc(battery_get_voltage_mv()));
+			platform_display_draw_string(0, DISPLAY_LINE_BATTERY_VOLTAGE + 3, tmpstr);
 		}
 		do_covid();
 		do_gatt();
diff --git a/zephyr/pinetime_devkit0.overlay b/zephyr/pinetime_devkit0.overlay
index 3fc6498..939ce5a 100644
--- a/zephyr/pinetime_devkit0.overlay
+++ b/zephyr/pinetime_devkit0.overlay
@@ -9,4 +9,11 @@
         output-ohms = <1000000>;
         full-ohms = <(1000000 + 1000000)>;
     };
+
+    battery {
+        compatible = "simple-battery";
+        /* state of charge, battery voltage mv pairs
+           eyeballed from various small LiPo datasheets */
+        soc-voltage-mv = <100 4200 80 3950 60 3800 40 3750 20 3700 0 3000>;
+    };
 };
\ No newline at end of file