From 2e1d6cfddde3a92ae7adb0eb8abfe4eb7fe2af90 Mon Sep 17 00:00:00 2001
From: bkleiner <b.kleiner@str8labs.com>
Date: Sat, 19 Nov 2022 22:42:02 +0100
Subject: [PATCH] osd: add dynamic element

---
 src/core/profile.c   |  2 ++
 src/driver/osd/osd.c |  9 ++++++
 src/driver/osd/osd.h |  1 +
 src/osd/render.c     | 68 ++++++++++++++++++++++++++++++++++++++++++++
 src/osd/render.h     |  1 +
 5 files changed, 81 insertions(+)

diff --git a/src/core/profile.c b/src/core/profile.c
index 26c6c0127..9c795095b 100644
--- a/src/core/profile.c
+++ b/src/core/profile.c
@@ -454,6 +454,7 @@ const profile_t default_profile = {
             ENCODE_OSD_ELEMENT(1, 0, 1, 14),  // OSD_CURRENT_DRAW
             ENCODE_OSD_ELEMENT(0, 0, 14, 6),  // OSD_CROSSHAIR
             ENCODE_OSD_ELEMENT(1, 0, 1, 13),  // OSD_CURRENT_DRAWN
+            ENCODE_OSD_ELEMENT(1, 0, 1, 2),   // OSD_DEBUG
         },
         .elements_hd = {
             ENCODE_OSD_ELEMENT(1, 1, 19, 0),  // OSD_CALLSIGN
@@ -470,6 +471,7 @@ const profile_t default_profile = {
             ENCODE_OSD_ELEMENT(1, 0, 0, 17),  // OSD_CURRENT_DRAW
             ENCODE_OSD_ELEMENT(0, 0, 15, 8),  // OSD_CROSSHAIR
             ENCODE_OSD_ELEMENT(1, 0, 0, 16),  // OSD_CURRENT_DRAWN
+            ENCODE_OSD_ELEMENT(1, 0, 0, 2),   // OSD_DEBUG
         },
     },
     .blackbox = {
diff --git a/src/driver/osd/osd.c b/src/driver/osd/osd.c
index fd9d4123d..cb02afc1c 100644
--- a/src/driver/osd/osd.c
+++ b/src/driver/osd/osd.c
@@ -181,6 +181,15 @@ void osd_start(uint8_t attr, uint8_t x, uint8_t y) {
   osd_seg.offset = 0;
 }
 
+uint32_t osd_auto_width(int32_t val) {
+  uint32_t width = 0;
+  while (val) {
+    val /= 10;
+    width++;
+  }
+  return width;
+}
+
 void osd_write_char(const char val) {
   osd_display_set(osd_seg.x + osd_seg.offset, osd_seg.y, osd_seg.attr, val);
   osd_seg.offset += 1;
diff --git a/src/driver/osd/osd.h b/src/driver/osd/osd.h
index f95f00efb..6061b19df 100644
--- a/src/driver/osd/osd.h
+++ b/src/driver/osd/osd.h
@@ -67,6 +67,7 @@ void osd_display_refresh();
 osd_system_t osd_check_system();
 
 void osd_start(uint8_t attr, uint8_t x, uint8_t y);
+uint32_t osd_auto_width(int32_t val);
 
 void osd_write_data(const uint8_t *buffer, uint8_t size);
 void osd_write_str(const char *buffer);
diff --git a/src/osd/render.c b/src/osd/render.c
index 2ec97f7af..3ba39aca2 100644
--- a/src/osd/render.c
+++ b/src/osd/render.c
@@ -71,6 +71,7 @@ static const char *osd_element_labels[] = {
     "CURRENT DRAW",
     "CROSSHAIR",
     "CURRENT DRAWN",
+    "DEBUG",
 };
 
 static const char *aux_channel_labels[] = {
@@ -290,6 +291,69 @@ static void print_osd_flightmode(osd_element_t *el) {
   osd_write_data(flightmode_labels[flightmode], 10);
 }
 
+static void print_state_member_uint8(uint8_t *data) {
+  osd_write_uint(*data, osd_auto_width(*data));
+}
+
+static void print_state_member_uint16(uint16_t *data) {
+  osd_write_uint(*data, osd_auto_width(*data));
+}
+
+static void print_state_member_uint32(uint32_t *data) {
+  osd_write_uint(*data, osd_auto_width(*data));
+}
+
+static void print_state_member_float(float *data) {
+  osd_write_float(*data, osd_auto_width(*data), 2);
+}
+
+static void print_state_member_vec3_t(vec3_t *data) {
+}
+
+static void print_state_member_vec4_t(vec4_t *data) {
+}
+
+static void print_state_member(char *name) {
+#define MEMBER(member, type)                  \
+  if (strcmp(name, #member) == 0) {           \
+    print_state_member_##type(&state.member); \
+    return;                                   \
+  }
+#define ARRAY_MEMBER(member, size, type) \
+  {}
+
+  STATE_MEMBERS
+
+#undef MEMBER
+#undef ARRAY_MEMBER
+}
+
+static void print_debug_element(osd_element_t *el, char *str) {
+  osd_start(osd_attr(el), el->pos_x, el->pos_y);
+
+  char element[32];
+  uint32_t element_len = 0;
+  bool started = false;
+  for (uint32_t i = 0; i < strlen(str); i++) {
+    if (str[i] == '{') {
+      element_len = 0;
+      started = true;
+      continue;
+    }
+    if (str[i] == '}') {
+      started = false;
+      element[element_len] = 0;
+      print_state_member(element);
+      continue;
+    }
+    if (started) {
+      element[element_len++] = str[i];
+    } else {
+      osd_write_char(str[i]);
+    }
+  }
+}
+
 static void print_osd_rssi(osd_element_t *el) {
   static float rx_rssi_filt;
   if (flags.failsafe)
@@ -499,7 +563,11 @@ static void osd_display_regular() {
     osd_start(osd_attr(el), el->pos_x, el->pos_y);
     osd_write_float(state.ibat_drawn, 4, 2);
     osd_write_char(ICON_MAH);
+    break;
+  }
 
+  case OSD_DEBUG: {
+    print_debug_element(el, "{cpu_load}S");
     osd_state.element++;
     break;
   }
diff --git a/src/osd/render.h b/src/osd/render.h
index 83af1de52..555f9df3c 100644
--- a/src/osd/render.h
+++ b/src/osd/render.h
@@ -70,6 +70,7 @@ typedef enum {
   OSD_CURRENT_DRAW,
   OSD_CROSSHAIR,
   OSD_CURRENT_DRAWN,
+  OSD_DEBUG,
 
   OSD_ELEMENT_MAX
 } osd_elements_t;