From 92d3b7ca6f7381b63e59447a0711e14baa9178c0 Mon Sep 17 00:00:00 2001
From: Alex Wolf <awolf78@gmx.de>
Date: Tue, 7 Aug 2018 21:06:54 -0400
Subject: [PATCH] GPS support via MSP

---
 src/main/interface/msp.c |  2 +-
 src/main/io/gps.c        | 13 ++++++++-----
 src/main/io/gps.h        |  7 ++++++-
 3 files changed, 15 insertions(+), 7 deletions(-)

diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index a7a00c103..9e896c18a 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -2027,7 +2027,7 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         gpsSol.llh.lon = sbufReadU32(src);
         gpsSol.llh.alt = sbufReadU16(src) * 100; // alt changed from 1m to 0.01m per lsb since MSP API 1.39 by RTH. Received MSP altitudes in 1m per lsb have to upscaled.
         gpsSol.groundSpeed = sbufReadU16(src);
-        GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
+        GPS_update |= GPS_MSP_UPDATE;        // MSP data signalisation to GPS functions
         break;
 #endif // USE_GPS
     case MSP_SET_FEATURE_CONFIG:
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index a0aca1f34..3b4dff8fd 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -98,7 +98,7 @@ static uint16_t fraction3[2];
 gpsSolutionData_t gpsSol;
 uint32_t GPS_packetCount = 0;
 uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received.
-uint8_t GPS_update = 0;             // it's a binary toggle to distinct a GPS position update
+uint8_t GPS_update = 0;             // toogle to distinct a GPS position update (directly or via MSP)
 
 uint8_t GPS_numCh;                          // Number of channels
 uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS];     // Channel number
@@ -492,6 +492,12 @@ void gpsUpdate(timeUs_t currentTimeUs)
     if (gpsPort) {
         while (serialRxBytesWaiting(gpsPort))
             gpsNewData(serialRead(gpsPort));
+    } else if (GPS_update & GPS_MSP_UPDATE) { // GPS data received via MSP
+        gpsSetState(GPS_RECEIVING_DATA);
+        gpsData.lastMessage = millis();
+        sensorsSet(SENSOR_GPS);
+        onGpsNewData();
+        GPS_update &= ~GPS_MSP_UPDATE;
     }
 
     switch (gpsData.state) {
@@ -547,10 +553,7 @@ static void gpsNewData(uint16_t c)
     gpsData.lastMessage = millis();
     sensorsSet(SENSOR_GPS);
 
-    if (GPS_update == 1)
-        GPS_update = 0;
-    else
-        GPS_update = 1;
+    GPS_update ^= GPS_DIRECT_TICK;
 
 #if 0
     debug[3] = GPS_update;
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index d9bbf9a65..1bf0d492b 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -137,10 +137,15 @@ typedef enum {
 } navigationMode_e;
 extern navigationMode_e nav_mode;          // Navigation mode
 
+typedef enum {
+    GPS_DIRECT_TICK = 1 << 0,
+    GPS_MSP_UPDATE = 1 << 1
+} gpsUpdateToggle_e;
+
 extern gpsData_t gpsData;
 extern gpsSolutionData_t gpsSol;
 
-extern uint8_t GPS_update;                 // it's a binary toogle to distinct a GPS position update
+extern uint8_t GPS_update;       // toogle to distinct a GPS position update (directly or via MSP)
 extern uint32_t GPS_packetCount;
 extern uint32_t GPS_svInfoReceivedCount;
 extern uint8_t GPS_numCh;                  // Number of channels