From 1a104a8f2aa412a50c01b9a6c20ebafa65c8c62b Mon Sep 17 00:00:00 2001
From: breadoven <56191411+breadoven@users.noreply.github.com>
Date: Sat, 12 Dec 2020 23:51:38 +0000
Subject: [PATCH] Display only geospatial WPs on OSD

Fix to ensure only geospatial WPs are displayed on OSD with sequential numbering maintained around nongeo WPs.
---
 src/main/io/osd.c                        | 15 ++++++++++++++-
 src/main/navigation/navigation.c         | 15 ++++++++++++++-
 src/main/navigation/navigation_private.h |  3 ++-
 3 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 0e6f9738b3..f5594c83e4 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -1383,6 +1383,19 @@ static void osdDisplayAdjustableDecimalValue(uint8_t elemPosX, uint8_t elemPosY,
     displayWriteWithAttr(osdDisplayPort, elemPosX + strlen(str) + 1 + valueOffset, elemPosY, buff, elemAttr);
 }
 
+int8_t getGeoWaypointNumber(int8_t waypointIndex)
+{
+    if (posControl.waypointList[waypointIndex].flag == NAV_WP_FLAG_LAST) {
+        if ((posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_JUMP || posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[waypointIndex].action == NAV_WP_ACTION_SET_HEAD)) {
+            return posControl.waypointList[waypointIndex - 1].flag;
+        } else {
+            return posControl.waypointList[waypointIndex - 1].flag + 1;
+        }
+    } else {
+        return posControl.waypointList[waypointIndex].flag;
+    }
+}
+
 static bool osdDrawSingleElement(uint8_t item)
 {
     uint16_t pos = osdLayoutsConfig()->item_pos[currentLayout][item];
@@ -3833,7 +3846,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                         // Countdown display for remaining Waypoints
                         char buf[6];
                         osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
-                        tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf);
+                        tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                         messages[messageCount++] = messageBuf;
                     } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                         // WP hold time countdown in seconds
diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
index 5bf4b26783..6917b20f7f 100644
--- a/src/main/navigation/navigation.c
+++ b/src/main/navigation/navigation.c
@@ -2833,13 +2833,25 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
         if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
+            static int8_t nonGeoWaypointCount = 0;
+            
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;
-                if(wpData->action == NAV_WP_ACTION_JUMP) {
+                if (wpData->action == NAV_WP_ACTION_JUMP) {
                     posControl.waypointList[wpNumber - 1].p1 -= 1; // make index (vice WP #)
+                    nonGeoWaypointCount += 1;
+                }
+                if (wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD) {
+                    nonGeoWaypointCount += 1;                          
                 }
                 posControl.waypointCount = wpNumber;
                 posControl.waypointListValid = (wpData->flag == NAV_WP_FLAG_LAST);
+                posControl.geoWaypointCount = posControl.waypointCount - nonGeoWaypointCount;
+                if (posControl.waypointListValid) {
+                    nonGeoWaypointCount = 0;
+                } else {                    
+                    posControl.waypointList[wpNumber - 1].flag = wpNumber - nonGeoWaypointCount;
+                }                                                                                             
             }
         }
     }
@@ -2851,6 +2863,7 @@ void resetWaypointList(void)
     if (!ARMING_FLAG(ARMED)) {
         posControl.waypointCount = 0;
         posControl.waypointListValid = false;
+        posControl.geoWaypointCount = 0;
     }
 }
 
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index b26299e707..308c55a9c7 100755
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -356,7 +356,8 @@ typedef struct {
     navWaypoint_t               waypointList[NAV_MAX_WAYPOINTS];
     bool                        waypointListValid;
     int8_t                      waypointCount;
-
+    int8_t                      geoWaypointCount;    // total geospatial WPs in mission
+    
     navWaypointPosition_t       activeWaypoint;     // Local position and initial bearing, filled on waypoint activation
     int8_t                      activeWaypointIndex;
     float                       wpInitialAltitude; // Altitude at start of WP