From 9792bf52d413ea1c9b8cbe9ee8d9cc8b6f9a7b8a Mon Sep 17 00:00:00 2001
From: 3djc <lesitewebdejc@hotmail.com>
Date: Sun, 14 Jul 2019 17:33:31 +0200
Subject: [PATCH] x12 gps improvement (#6570)

* Add hdop handling

* Add GPS to debug screen

* Only update PilotPos when a decent hdop is achieved
---
 radio/src/gps.cpp                         |  5 +++++
 radio/src/gps.h                           |  1 +
 radio/src/gui/480x272/view_statistics.cpp | 12 +++++++++++-
 radio/src/lua/api_general.cpp             |  2 ++
 radio/src/targets/horus/board.h           |  3 +++
 radio/src/telemetry/telemetry_sensors.cpp |  4 ++--
 6 files changed, 24 insertions(+), 3 deletions(-)

diff --git a/radio/src/gps.cpp b/radio/src/gps.cpp
index 5a62fb5ac..5ef79985f 100644
--- a/radio/src/gps.cpp
+++ b/radio/src/gps.cpp
@@ -134,6 +134,7 @@ typedef struct gpsDataNmea_s
   uint16_t altitude;
   uint16_t speed;
   uint16_t groundCourse;
+  uint16_t hdop;
   uint32_t date;
   uint32_t time;
 } gpsDataNmea_t;
@@ -207,6 +208,9 @@ bool gpsNewFrameNMEA(char c)
             case 7:
               gps_Msg.numSat = grab_fields(string, 0);
               break;
+            case 8:
+              gps_Msg.hdop = grab_fields(string, 1) * 10;
+              break;
             case 9:
               gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
               break;
@@ -258,6 +262,7 @@ bool gpsNewFrameNMEA(char c)
               frameOK = 1;
               gpsData.fix = gps_Msg.fix;
               gpsData.numSat = gps_Msg.numSat;
+              gpsData.hdop = gps_Msg.hdop;
               if (gps_Msg.fix) {
                 __disable_irq();    // do the atomic update of lat/lon
                 gpsData.latitude = gps_Msg.latitude;
diff --git a/radio/src/gps.h b/radio/src/gps.h
index 5d8bd9c19..22ba6c209 100644
--- a/radio/src/gps.h
+++ b/radio/src/gps.h
@@ -35,6 +35,7 @@ struct gpsdata_t
   uint16_t altitude;              // altitude in 0.1m
   uint16_t speed;                 // speed in 0.1m/s
   uint16_t groundCourse;          // degrees * 10
+  uint16_t hdop;
 };
 
 extern gpsdata_t gpsData;
diff --git a/radio/src/gui/480x272/view_statistics.cpp b/radio/src/gui/480x272/view_statistics.cpp
index 1c40bad8d..98eae593c 100644
--- a/radio/src/gui/480x272/view_statistics.cpp
+++ b/radio/src/gui/480x272/view_statistics.cpp
@@ -168,7 +168,17 @@ bool menuStatsDebug(event_t event)
 
   lcdDrawText(MENUS_MARGIN_LEFT, y, "Telem RX Errs");
   lcdDrawNumber(MENU_STATS_COLUMN1, y, telemetryErrors, LEFT);
-  // y += FH;
+  y += FH;
+
+#if defined(INTERNAL_GPS)
+  lcdDrawText(MENUS_MARGIN_LEFT, y, "Internal GPS");
+  lcdDrawText(MENU_STATS_COLUMN1, y+1, "[Fix]", HEADER_COLOR|SMLSIZE);
+  lcdDrawText(lcdNextPos+2, y, (gpsData.fix ? "Yes" : "No"), LEFT);
+  lcdDrawText(lcdNextPos+20, y+1, "[Sats]", HEADER_COLOR|SMLSIZE);
+  lcdDrawNumber(lcdNextPos+5, y, gpsData.numSat, LEFT);
+  lcdDrawText(lcdNextPos+20, y+1, "[Hdop]", HEADER_COLOR|SMLSIZE);
+  lcdDrawNumber(lcdNextPos+5, y, gpsData.hdop, PREC2|LEFT);
+#endif
 
   lcdDrawText(LCD_W/2, MENU_FOOTER_TOP, STR_MENUTORESET, MENU_TITLE_COLOR | CENTERED);
   return true;
diff --git a/radio/src/lua/api_general.cpp b/radio/src/lua/api_general.cpp
index feae278a1..1f2dcbe17 100644
--- a/radio/src/lua/api_general.cpp
+++ b/radio/src/lua/api_general.cpp
@@ -806,6 +806,7 @@ Return the internal GPS position or nil if no valid hardware found
  * 'alt' (number) internal GPS altitude in 0.1m
  * 'speed' (number) internal GPSspeed in 0.1m/s
  * 'heading'  (number) internal GPS ground course estimation in degrees * 10
+ * 'hdop' (number)  internal GPS horizontal dilution of precision
 
 @status current Introduced in 2.2.2
 */
@@ -819,6 +820,7 @@ static int luaGetTxGPS(lua_State * L)
   lua_pushtableinteger(L, "alt", gpsData.altitude);
   lua_pushtableinteger(L, "speed", gpsData.speed);
   lua_pushtableinteger(L, "heading", gpsData.groundCourse);
+  lua_pushtableinteger(L, "hdop", gpsData.hdop);
   if (gpsData.fix)
     lua_pushtableboolean(L, "fix", true);
   else
diff --git a/radio/src/targets/horus/board.h b/radio/src/targets/horus/board.h
index 9c7dff7ef..8853af830 100644
--- a/radio/src/targets/horus/board.h
+++ b/radio/src/targets/horus/board.h
@@ -547,6 +547,9 @@ uint8_t gpsGetByte(uint8_t * byte);
 extern uint8_t gpsTraceEnabled;
 #endif
 void gpsSendByte(uint8_t byte);
+#if defined(PCBX12S)
+#define PILOTPOS_MIN_HDOP             500
+#endif
 
 // Second serial port driver
 #define AUX_SERIAL
diff --git a/radio/src/telemetry/telemetry_sensors.cpp b/radio/src/telemetry/telemetry_sensors.cpp
index fe97a0397..8e4e72829 100644
--- a/radio/src/telemetry/telemetry_sensors.cpp
+++ b/radio/src/telemetry/telemetry_sensors.cpp
@@ -141,7 +141,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
   }
   else if (unit == UNIT_GPS_LATITUDE) {
 #if defined(INTERNAL_GPS)
-    if (gpsData.fix) {
+    if (gpsData.fix  && gpsData.hdop < PILOTPOS_MIN_HDOP) {
       pilotLatitude = gpsData.latitude;
       distFromEarthAxis = getDistFromEarthAxis(pilotLatitude);
     }
@@ -156,7 +156,7 @@ void TelemetryItem::setValue(const TelemetrySensor & sensor, int32_t val, uint32
   }
   else if (unit == UNIT_GPS_LONGITUDE) {
 #if defined(INTERNAL_GPS)
-    if (gpsData.fix) {
+    if (gpsData.fix && gpsData.hdop < PILOTPOS_MIN_HDOP) {
       pilotLongitude = gpsData.longitude;
     }
 #endif