From 0686b7419700e0940eba014fa61de2c2ee784d97 Mon Sep 17 00:00:00 2001
From: Mark Haslinghuis <mark@numloq.nl>
Date: Wed, 26 Jun 2024 22:19:06 +0200
Subject: [PATCH] Add altitude variant for OSD (#13716)

---
 src/main/flight/position.c             |  7 +++++++
 src/main/flight/position.h             |  1 +
 src/main/osd/osd_elements.c            | 29 ++++++++++++++++----------
 src/test/unit/link_quality_unittest.cc |  1 +
 src/test/unit/osd_unittest.cc          |  4 ++++
 5 files changed, 31 insertions(+), 11 deletions(-)

diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index 4c8f320810..8552f3d766 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -223,6 +223,13 @@ float getAltitude(void)
     return zeroedAltitudeCm;
 }
 
+#ifdef USE_GPS
+float getAltitudeAsl(void)
+{
+    return gpsSol.llh.altCm;
+}
+#endif
+
 #ifdef USE_VARIO
 int16_t getEstimatedVario(void)
 {
diff --git a/src/main/flight/position.h b/src/main/flight/position.h
index 3a3faacdb5..8084b4c97f 100644
--- a/src/main/flight/position.h
+++ b/src/main/flight/position.h
@@ -37,4 +37,5 @@ void calculateEstimatedAltitude(void);
 void positionInit(void);
 int32_t getEstimatedAltitudeCm(void);
 float getAltitude(void);
+float getAltitudeAsl(void);
 int16_t getEstimatedVario(void);
diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c
index 31cc51448e..fc3df7bc27 100644
--- a/src/main/osd/osd_elements.c
+++ b/src/main/osd/osd_elements.c
@@ -318,19 +318,26 @@ int osdConvertTemperatureToSelectedUnit(int tempInDegreesCelcius)
 
 static void osdFormatAltitudeString(char * buff, int32_t altitudeCm, osdElementType_e variantType)
 {
-    const char unitSymbol = osdGetMetersToSelectedUnitSymbol();
-    unsigned decimalPlaces;
+    static const struct {
+        uint8_t decimals; 
+        bool asl;
+    } variantMap[] = {
+        [OSD_ELEMENT_TYPE_1] = { 1, false },
+        [OSD_ELEMENT_TYPE_2] = { 0, false },
+        [OSD_ELEMENT_TYPE_3] = { 1, true },
+        [OSD_ELEMENT_TYPE_4] = { 0, true },
+    };
 
-    switch (variantType) {
-    case OSD_ELEMENT_TYPE_2:  // whole number altitude (no decimal places)
-        decimalPlaces = 0;
-        break;
-    case OSD_ELEMENT_TYPE_1:  // one decimal place (default)
-    default:
-        decimalPlaces = 1;
-        break;
+    int32_t alt = altitudeCm;
+#ifdef USE_GPS
+    if (variantMap[variantType].asl) {
+        alt = getAltitudeAsl();
     }
-    osdPrintFloat(buff, SYM_ALTITUDE, osdGetMetersToSelectedUnit(altitudeCm) / 100.0f, "", decimalPlaces, true, unitSymbol);
+#endif
+    unsigned decimalPlaces = variantMap[variantType].decimals;
+    const char unitSymbol = osdGetMetersToSelectedUnitSymbol();
+
+    osdPrintFloat(buff, SYM_ALTITUDE, osdGetMetersToSelectedUnit(alt) / 100.0f, "", decimalPlaces, true, unitSymbol);
 }
 
 #ifdef USE_GPS
diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc
index 29f099387b..b61a29caec 100644
--- a/src/test/unit/link_quality_unittest.cc
+++ b/src/test/unit/link_quality_unittest.cc
@@ -482,6 +482,7 @@ extern "C" {
     int32_t getMAhDrawn() { return 0; }
     float getWhDrawn() { return 0.0; }
     int32_t getEstimatedAltitudeCm() { return 0; }
+    int32_t getAltitudeAsl() { return 0; }
     int32_t getEstimatedVario() { return 0; }
     int32_t blackboxGetLogNumber() { return 0; }
     bool isBlackboxDeviceWorking() { return true; }
diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc
index d23b295439..7336ea3829 100644
--- a/src/test/unit/osd_unittest.cc
+++ b/src/test/unit/osd_unittest.cc
@@ -1357,6 +1357,10 @@ extern "C" {
         return simulationAltitude;
     }
 
+    int32_t getAltitudeAsl() {
+        return simulationAltitude;
+    }
+
     int32_t getEstimatedVario() {
         return simulationVerticalSpeed;
     }