From b370cec53712deadc9a573032e5f48c0da47b546 Mon Sep 17 00:00:00 2001
From: Martin Budden <mjbudden@gmail.com>
Date: Wed, 29 Mar 2017 15:31:15 +0100
Subject: [PATCH] Added GPS position to OSD

---
 src/main/fc/cli.c |  2 ++
 src/main/io/osd.c | 39 ++++++++++++++++++++++++++++++++++-----
 src/main/io/osd.h |  2 ++
 3 files changed, 38 insertions(+), 5 deletions(-)

diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c
index 4aa17e24e5..100205c1a9 100755
--- a/src/main/fc/cli.c
+++ b/src/main/fc/cli.c
@@ -838,6 +838,8 @@ static const clivalue_t valueTable[] = {
     { "osd_mah_drawn_pos",          VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_MAH_DRAWN]) },
     { "osd_craft_name_pos",         VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_CRAFT_NAME]) },
     { "osd_gps_speed_pos",          VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SPEED]) },
+    { "osd_gps_lon",                VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_LON]) },
+    { "osd_gps_lat",                VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_LAT]) },
     { "osd_gps_sats_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SATS]) },
     { "osd_altitude_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ALTITUDE]) },
     { "osd_pid_roll_pos",           VAR_UINT16  | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_PIDS]) },
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 597b3f9cd0..995fea8c3f 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -39,8 +39,12 @@
 #include "build/debug.h"
 #include "build/version.h"
 
-#include "common/printf.h"
+#include "cms/cms.h"
+#include "cms/cms_types.h"
+#include "cms/cms_menu_osd.h"
+
 #include "common/maths.h"
+#include "common/printf.h"
 #include "common/typeconversion.h"
 #include "common/utils.h"
 
@@ -59,10 +63,6 @@
 #include "drivers/vtx_rtc6705.h"
 #endif
 
-#include "cms/cms.h"
-#include "cms/cms_types.h"
-#include "cms/cms_menu_osd.h"
-
 #include "io/asyncfatfs/asyncfatfs.h"
 #include "io/flashfs.h"
 #include "io/gps.h"
@@ -228,6 +228,30 @@ static void osdDrawSingleElement(uint8_t item)
             sprintf(buff, "%dK", CM_S_TO_KM_H(GPS_speed) * 10);
             break;
         }
+
+        case OSD_GPS_LAT:
+        case OSD_GPS_LON:
+        {
+            int32_t val;
+            if (item == OSD_GPS_LAT) {
+                buff[0] = 0xA6;
+                val = GPS_coord[LAT];
+            } else {
+                buff[0] = 0xA7;
+                val = GPS_coord[LON];
+            }
+            if (val >= 0) {
+                val += 1000000000;
+            } else {
+                val -= 1000000000;
+            }
+            itoa(val, &buff[1], 10);
+            buff[1] = buff[2];
+            buff[2] = buff[3];
+            buff[3] = '.';
+            break;
+        }
+
 #endif // GPS
 
         case OSD_ALTITUDE:
@@ -503,6 +527,8 @@ void osdDrawElements(void)
     {
         osdDrawSingleElement(OSD_GPS_SATS);
         osdDrawSingleElement(OSD_GPS_SPEED);
+        osdDrawSingleElement(OSD_GPS_LAT);
+        osdDrawSingleElement(OSD_GPS_LON);
     }
 #endif // GPS
 }
@@ -532,6 +558,9 @@ void pgResetFn_osdConfig(osdConfig_t *osdProfile)
     osdProfile->item_pos[OSD_MAIN_BATT_WARNING] = OSD_POS(9, 10) | VISIBLE_FLAG;
     osdProfile->item_pos[OSD_AVG_CELL_VOLTAGE] = OSD_POS(12, 2) | VISIBLE_FLAG;
 
+    osdProfile->item_pos[OSD_GPS_LAT] = OSD_POS(18, 14) | VISIBLE_FLAG;
+    osdProfile->item_pos[OSD_GPS_LON] = OSD_POS(18, 15) | VISIBLE_FLAG;
+
     osdProfile->units = OSD_UNIT_METRIC;
     osdProfile->rssi_alarm = 20;
     osdProfile->cap_alarm = 2200;
diff --git a/src/main/io/osd.h b/src/main/io/osd.h
index 60fd65963a..ee26fb898a 100755
--- a/src/main/io/osd.h
+++ b/src/main/io/osd.h
@@ -49,6 +49,8 @@ typedef enum {
     OSD_PIDRATE_PROFILE,
     OSD_MAIN_BATT_WARNING,
     OSD_AVG_CELL_VOLTAGE,
+    OSD_GPS_LON,
+    OSD_GPS_LAT,
     OSD_ITEM_COUNT // MUST BE LAST
 } osd_items_e;