It works!!!

src/core/gfx.cpp

@@ -0,0 +1,74 @@
+#include "config.hpp"
+#include "hal/draw.hpp"
+#include <array>
+#include <cmath>
+#include <cstdint>
+
+struct color_stop {
+  double value;
+  draw::color_t color;
+};
+
+draw::color_t interpolateColor(double value,
+                               const std::array<color_stop, 11> &stops) {
+  if (value <= stops.front().value)
+    return stops.front().color;
+
+  if (value >= stops.back().value)
+    return stops.back().color;
+
+  for (size_t i = 0; i < stops.size() - 1; ++i) {
+    if (value >= stops[i].value && value <= stops[i + 1].value) {
+
+      double t =
+          (value - stops[i].value) / (stops[i + 1].value - stops[i].value);
+
+      draw::color_t c0 = stops[i].color;
+      draw::color_t c1 = stops[i + 1].color;
+
+      draw::color_t result;
+      result.r = static_cast<uint8_t>(std::round(c0.r + t * (c1.r - c0.r)));
+      result.g = static_cast<uint8_t>(std::round(c0.g + t * (c1.g - c0.g)));
+      result.b = static_cast<uint8_t>(std::round(c0.b + t * (c1.b - c0.b)));
+
+      return result;
+    }
+  }
+
+  // Fallback
+  return stops.back().color;
+}
+
+#ifdef METRIC
+draw::color_t get_altitude_color(uint32_t value) {
+  static constexpr std::array<color_stop, 11> stops = {{
+#ifdef METRIC
+      {0, {220, 90, 20}},
+      {150, {245, 115, 25}},
+      {300, {250, 150, 30}},
+      {600, {245, 180, 20}},
+      {1200, {225, 200, 20}},
+      {1800, {175, 215, 30}},
+      {2400, {100, 225, 50}},
+      {3000, {40, 210, 110}},
+      {6000, {40, 175, 215}},
+      {9000, {50, 110, 240}},
+      {12000, {140, 50, 220}}
+#endif
+#ifndef METRIC
+      {0, {220, 90, 20}},      // Brown/Dark Orange
+      {500, {245, 115, 25}},   // Orange
+      {1000, {250, 150, 30}},  // Light Orange
+      {2000, {245, 180, 20}},  // Yellow-Orange
+      {4000, {225, 200, 20}},  // Yellow
+      {6000, {175, 215, 30}},  // Yellow-Green
+      {8000, {100, 225, 50}},  // Light Green
+      {10000, {40, 210, 110}}, // Teal/Cyan
+      {20000, {40, 175, 215}}, // Cyan-Blue
+      {30000, {50, 110, 240}}, // Blue
+      {40000, {140, 50, 220}}  // Purple
+#endif
+  }};
+  return interpolateColor(value, stops);
+}
+#endif