3 files changed, 51 insertions, 36 deletions

diff --git a/src/components/battery/BatteryController.cpp b/src/components/battery/BatteryController.cpp
index 206b26ed..b61f0ce3 100644
--- a/src/components/battery/BatteryController.cpp
+++ b/src/components/battery/BatteryController.cpp
@@ -1,4 +1,5 @@
 #include "components/battery/BatteryController.h"
+#include "components/utility/LinearApproximation.h"
 #include "drivers/PinMap.h"
 #include <hal/nrf_gpio.h>
 #include <nrfx_saadc.h>
@@ -60,6 +61,14 @@ void Battery::SaadcInit() {
 }
 
 void Battery::SaadcEventHandler(nrfx_saadc_evt_t const* p_event) {
+  static const Utility::LinearApproximation<uint16_t, uint8_t, 6> aprox {{{
+    {3500, 0},  // Minimum voltage before shutdown (depends on the battery)
+    {3600, 10}, // Keen point that corresponds to 10%
+    {3700, 25},
+    {3750, 50},
+    {3900, 75},
+    {4180, 100} // Maximum voltage during charging is 4.21V
+  }}};
 
   if (p_event->type == NRFX_SAADC_EVT_DONE) {
 
@@ -76,7 +85,7 @@ void Battery::SaadcEventHandler(nrfx_saadc_evt_t const* p_event) {
     if (isFull) {
       newPercent = 100;
     } else {
-      newPercent = std::min(GetBatteryPercentageFromVoltage(voltage), static_cast<uint8_t>(isCharging ? 99 : 100));
+      newPercent = std::min(aprox.GetValue(voltage), isCharging ? uint8_t {99} : uint8_t {100});
     }
 
     if ((isPowerPresent && newPercent > percentRemaining) || (!isPowerPresent && newPercent < percentRemaining) || firstMeasurement) {
@@ -93,37 +102,3 @@ void Battery::SaadcEventHandler(nrfx_saadc_evt_t const* p_event) {
 void Battery::Register(Pinetime::System::SystemTask* systemTask) {
   this->systemTask = systemTask;
 }
-
-uint8_t Battery::GetBatteryPercentageFromVoltage(uint16_t voltage) {
-  // The number of line segments used to approximate the battery discharge curve.
-  static const uint8_t LINE_SEGMENT_COUNT = 7;
-
-  // The voltages (mV) at the endpoints of the line segments. Any two consecutive
-  // values represent the start and end voltage of a line segment.
-  static const uint16_t voltageOffsets[LINE_SEGMENT_COUNT + 1] {4157, 4063, 3882, 3747, 3716, 3678, 3583, 3500};
-
-  // The battery percentages at the endpoints of the line segments. Note that last
-  // value is omitted: It is not needed because we only need the percentages at
-  // the start of each line segment.
-  static const float percentageOffsets[LINE_SEGMENT_COUNT] {100.000, 95.197, 70.429, 48.947, 35.158, 18.971, 5.801};
-
-  // The pre-calculated slopes (in battery percentage points per millivolt) of the
-  // line segments.
-  static const float percentageSlopes[LINE_SEGMENT_COUNT] {0.05109, 0.13684, 0.15913, 0.44481, 0.42595, 0.13863, 0.06989};
-
-  if (voltage >= voltageOffsets[0]) {
-    return 100;
-  }
-
-  if (voltage <= voltageOffsets[7]) {
-    return 0;
-  }
-
-  for (uint8_t i = 0; i < LINE_SEGMENT_COUNT; i++) {
-    if (voltage > voltageOffsets[i + 1]) {
-      return static_cast<uint8_t>(roundf(percentageOffsets[i] + percentageSlopes[i] * (voltage - voltageOffsets[i])));
-    }
-  }
-
-  return 0;
-}
diff --git a/src/components/battery/BatteryController.h b/src/components/battery/BatteryController.h
index 15559916..5a7394c4 100644
--- a/src/components/battery/BatteryController.h
+++ b/src/components/battery/BatteryController.h
@@ -49,7 +49,6 @@ namespace Pinetime {
 
       void SaadcEventHandler(nrfx_saadc_evt_t const* p_event);
       static void AdcCallbackStatic(nrfx_saadc_evt_t const* event);
-      static uint8_t GetBatteryPercentageFromVoltage(uint16_t voltage);
 
       bool isReading = false;
 
diff --git a/src/components/utility/LinearApproximation.h b/src/components/utility/LinearApproximation.h
new file mode 100644
index 00000000..f7104ced
--- /dev/null
+++ b/src/components/utility/LinearApproximation.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include <cstddef>
+#include <array>
+
+namespace Pinetime {
+  namespace Utility {
+
+    // based on: https://github.com/SHristov92/LinearApproximation/blob/main/Linear.h
+    template <typename Key, typename Value, std::size_t Size> class LinearApproximation {
+      using Point = struct {
+        Key key;
+        Value value;
+      };
+
+    public:
+      LinearApproximation(const std::array<Point, Size>&& sorted_points) : points {sorted_points} {
+      }
+
+      Value GetValue(Key key) const {
+        if (key <= points[0].key) {
+          return points[0].value;
+        }
+
+        for (std::size_t i = 1; i < Size; i++) {
+          const auto& p = points[i];
+          const auto& p_prev = points[i - 1];
+
+          if (key < p.key) {
+            return p_prev.value + (key - p_prev.key) * (p.value - p_prev.value) / (p.key - p_prev.key);
+          }
+        }
+
+        return points[Size - 1].value;
+      }
+
+    private:
+      std::array<Point, Size> points;
+    };
+  }
+}