From 7115ab404749626770bf37fd905b1e8536886518 Mon Sep 17 00:00:00 2001
From: Alex Wolf <awolf78@gmx.de>
Date: Tue, 28 Aug 2018 14:57:01 -0400
Subject: [PATCH] GPS over MSP related fixes

---
 src/main/fc/config.c          | 13 ++++++++++---
 src/main/interface/settings.c |  2 +-
 src/main/io/gps.c             | 11 +++++++++++
 src/main/io/gps.h             |  3 ++-
 src/main/telemetry/crsf.c     |  3 ++-
 5 files changed, 26 insertions(+), 6 deletions(-)

diff --git a/src/main/fc/config.c b/src/main/fc/config.c
index a980902e4f..c40e1ffc16 100644
--- a/src/main/fc/config.c
+++ b/src/main/fc/config.c
@@ -48,6 +48,7 @@
 #include "io/beeper.h"
 #include "io/ledstrip.h"
 #include "io/serial.h"
+#include "io/gps.h"
 
 #include "pg/beeper.h"
 #include "pg/beeper_dev.h"
@@ -163,9 +164,15 @@ static void validateAndFixConfig(void)
         pgResetFn_serialConfig(serialConfigMutable());
     }
 
+#if defined(USE_GPS)
+    serialPortConfig_t *gpsSerial = findSerialPortConfig(FUNCTION_GPS);
+    if (gpsConfig()->provider == GPS_MSP && gpsSerial) {
+        serialRemovePort(gpsSerial->identifier);
+    }
+#endif
     if (
 #if defined(USE_GPS)
-        !findSerialPortConfig(FUNCTION_GPS) &&
+        gpsConfig()->provider != GPS_MSP && !gpsSerial &&
 #endif
         true) {
         featureDisable(FEATURE_GPS);
@@ -271,7 +278,7 @@ static void validateAndFixConfig(void)
             pidProfilesMutable(i)->pid[PID_YAW].F = 0;
         }
     }
-    
+
 #if defined(USE_THROTTLE_BOOST)
     if (!rcSmoothingIsEnabled() ||
         !(rxConfig()->rcInterpolationChannels == INTERPOLATION_CHANNELS_RPYT
@@ -409,7 +416,7 @@ void validateAndFixGyroConfig(void)
         featureDisable(FEATURE_DYNAMIC_FILTER);
     }
 #endif
-    
+
     // Prevent invalid notch cutoff
     if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
         gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 17e45a79c9..aecaac6473 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -166,7 +166,7 @@ static const char * const lookupTableGyro[] = {
 
 #ifdef USE_GPS
 static const char * const lookupTableGPSProvider[] = {
-    "NMEA", "UBLOX"
+    "NMEA", "UBLOX", "MSP"
 };
 
 static const char * const lookupTableGPSSBASMode[] = {
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index dd7eaa16d5..0f9c99b018 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -224,6 +224,7 @@ static const uint8_t ubloxGalileoInit[] = {
 typedef enum {
     GPS_UNKNOWN,
     GPS_INITIALIZING,
+    GPS_INITIALIZED,
     GPS_CHANGE_BAUD,
     GPS_CONFIGURE,
     GPS_RECEIVING_DATA,
@@ -280,6 +281,11 @@ void gpsInit(void)
     gpsSetState(GPS_UNKNOWN);
 
     gpsData.lastMessage = millis();
+    
+    if (gpsConfig()->provider == GPS_MSP) { // no serial ports used when GPS_MSP is configured
+        gpsSetState(GPS_INITIALIZED);
+        return;
+    }
 
     serialPortConfig_t *gpsPortConfig = findSerialPortConfig(FUNCTION_GPS);
     if (!gpsPortConfig) {
@@ -474,6 +480,8 @@ void gpsInitHardware(void)
         gpsInitUblox();
 #endif
         break;
+    default:
+        break;
     }
 }
 
@@ -502,6 +510,7 @@ void gpsUpdate(timeUs_t currentTimeUs)
 
     switch (gpsData.state) {
         case GPS_UNKNOWN:
+        case GPS_INITIALIZED:
             break;
 
         case GPS_INITIALIZING:
@@ -575,6 +584,8 @@ bool gpsNewFrame(uint8_t c)
         return gpsNewFrameUBLOX(c);
 #endif
         break;
+    default:
+        break;
     }
     return false;
 }
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 70768cbe41..f5e257be6e 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -34,7 +34,8 @@
 
 typedef enum {
     GPS_NMEA = 0,
-    GPS_UBLOX
+    GPS_UBLOX,
+    GPS_MSP
 } gpsProvider_e;
 
 typedef enum {
diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c
index f789b81fbf..12b7263c7d 100644
--- a/src/main/telemetry/crsf.c
+++ b/src/main/telemetry/crsf.c
@@ -449,11 +449,12 @@ void initCrsfTelemetry(void)
         crsfSchedule[index++] = BV(CRSF_FRAME_BATTERY_SENSOR_INDEX);
     }
     crsfSchedule[index++] = BV(CRSF_FRAME_FLIGHT_MODE_INDEX);
+#ifdef USE_GPS
     if (featureIsEnabled(FEATURE_GPS)) {
         crsfSchedule[index++] = BV(CRSF_FRAME_GPS_INDEX);
     }
+#endif
     crsfScheduleCount = (uint8_t)index;
-
  }
 
 bool checkCrsfTelemetryState(void)