From f7cf3db93d925833f40f03f1fc420142b4d39f80 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Thu, 16 Sep 2021 14:41:11 -0600
Subject: [PATCH 01/17] Increase firmware version

---
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index 57f6b8713..e358b8d31 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -39,7 +39,7 @@
 */
 
 const int FIRMWARE_VERSION_MAJOR = 1;
-const int FIRMWARE_VERSION_MINOR = 5;
+const int FIRMWARE_VERSION_MINOR = 6;
 
 #define COMPILE_WIFI //Comment out to remove all WiFi functionality
 #define COMPILE_BT //Comment out to disable all Bluetooth

From 69989bc838d87b43b369fcf23668ef179820aad1 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Fri, 17 Sep 2021 15:30:14 -0600
Subject: [PATCH 02/17] Move EEPROM settings access so we can check
 enableResetDisplay setting. Move resetCount to purely NVM (no file).

---
 Firmware/RTK_Surveyor/Begin.ino        | 22 +++++++++++++++++-----
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 18 ++++++------------
 Firmware/RTK_Surveyor/Tasks.ino        |  2 +-
 Firmware/RTK_Surveyor/menuDebug.ino    | 11 +++++++++++
 4 files changed, 35 insertions(+), 18 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino
index 64bf5a7e7..0f8b12ba7 100644
--- a/Firmware/RTK_Surveyor/Begin.ino
+++ b/Firmware/RTK_Surveyor/Begin.ino
@@ -38,7 +38,7 @@ void beginBoard()
 
     //Bug in ZED-F9P v1.13 firmware causes RTK LED to not light when RTK Floating with SBAS on.
     //The following changes the POR default but will be overwritten by settings in NVM or settings file
-    ubxConstellations[1].enabled = false; 
+    ubxConstellations[1].enabled = false;
 
     strcpy(platformFilePrefix, "SFE_Surveyor");
     strcpy(platformPrefix, "Surveyor");
@@ -112,14 +112,25 @@ void beginBoard()
   if (esp_reset_reason() == ESP_RST_POWERON)
   {
     reuseLastLog = false; //Start new log
-    settings.resetCount = 0;
-    recordSystemSettings(); //Record to NVM
+
+    loadSettingsPartial();
+    if (settings.enableResetDisplay == true)
+    {
+      settings.resetCount = 0;
+      recordSystemSettings(); //Record to NVM
+    }
   }
   else
   {
     reuseLastLog = true; //Attempt to reuse previous log
-    settings.resetCount++;
-    recordSystemSettings(); //Record to NVM
+
+    loadSettingsPartial();
+    if (settings.enableResetDisplay == true)
+    {
+      settings.resetCount++;
+      recordSystemSettings(); //Record to NVM
+      Serial.printf("resetCount: %d\n\r", settings.resetCount);
+    }
 
     Serial.print("Reset reason: ");
     switch (esp_reset_reason())
@@ -336,6 +347,7 @@ void beginGNSS()
 
     printModuleInfo(); //Print module type and firmware version
   }
+
   online.gnss = true;
 }
 
diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index e358b8d31..f4c397392 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -147,10 +147,6 @@ uint32_t casterResponseWaitStartTime = 0; //Used to detect if caster service tim
 //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
 #include <SparkFun_u-blox_GNSS_Arduino_Library.h> //http://librarymanager/All#SparkFun_u-blox_GNSS
 
-//Note: There are two prevalent versions of the ZED-F9P: v1.12 (part# -01B) and v1.13 (-02B).
-//v1.13 causes the RTK LED to not function if SBAS is enabled. To avoid this, we
-//disable SBAS by default.
-
 char zedFirmwareVersion[20]; //The string looks like 'HPG 1.12'. Output to debug menu and settings file.
 uint8_t zedModuleType = PLATFORM_F9P; //Controls which messages are supported and configured
 
@@ -221,7 +217,7 @@ uint8_t rBuffer[SERIAL_SIZE_RX]; //Buffer for reading from F9P to SPP
 uint8_t wBuffer[SERIAL_SIZE_RX]; //Buffer for writing from incoming SPP to F9P
 TaskHandle_t F9PSerialReadTaskHandle = NULL; //Store handles so that we can kill them if user goes into WiFi NTRIP Server mode
 TaskHandle_t F9PSerialWriteTaskHandle = NULL; //Store handles so that we can kill them if user goes into WiFi NTRIP Server mode
-const uint8_t F9PSerialWriteTaskPriority = 1; //Priority, with 3 being the highest, and 0 being the lowest.
+const uint8_t F9PSerialWriteTaskPriority = 1; //3 being the highest, and 0 being the lowest.
 const uint8_t F9PSerialReadTaskPriority = 1;
 
 TaskHandle_t pinUART2TaskHandle = NULL; //Dummy task to start UART2 on core 0.
@@ -275,7 +271,7 @@ Button *setupBtn = NULL; //We can't instantiate the buttons here because we don'
 Button *powerBtn = NULL;
 
 TaskHandle_t ButtonCheckTaskHandle = NULL;
-const uint8_t ButtonCheckTaskPriority = 1; //Priority, with 3 being the highest, and 0 being the lowest.
+const uint8_t ButtonCheckTaskPriority = 1; //3 being the highest, and 0 being the lowest.
 const int buttonTaskStackSize = 2000;
 
 const int shutDownButtonTime = 2000; //ms press and hold before shutdown
@@ -360,20 +356,19 @@ void setup()
   Serial.begin(115200); //UART0 for programming and debugging
 
   Wire.begin(); //Start I2C on core 1
-  Wire.setClock(400000);
+  //Wire.setClock(400000);
+  Wire.setClock(100000);
 
   beginGNSS(); //Connect to GNSS
 
+  beginEEPROM(); //Start EEPROM and SD for settings
+
   beginBoard(); //Determine what hardware platform we are running on
 
   beginDisplay(); //Check if an external Qwiic OLED is attached
 
   beginLEDs(); //LED and PWM setup
 
-  //Start EEPROM and SD for settings, and display for output
-  //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
-  beginEEPROM();
-
   //eepromErase();
 
   beginSD(); //Test if SD is present
@@ -384,7 +379,6 @@ void setup()
   }
 
   loadSettings(); //Attempt to load settings after SD is started so we can read the settings file if available
-  //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
 
   beginUART2(); //Start UART2 on core 0, used to receive serial from ZED and pass out over SPP
 
diff --git a/Firmware/RTK_Surveyor/Tasks.ino b/Firmware/RTK_Surveyor/Tasks.ino
index 3cfbb131e..b2d7049ac 100644
--- a/Firmware/RTK_Surveyor/Tasks.ino
+++ b/Firmware/RTK_Surveyor/Tasks.ino
@@ -1,4 +1,4 @@
-//High frequency tasks made by createTask()
+//High frequency tasks made by xTaskCreate()
 //And any low frequency tasks that are called by Ticker
 
 //If the phone has any new data (NTRIP RTCM, etc), read it in over Bluetooth and pass along to ZED
diff --git a/Firmware/RTK_Surveyor/menuDebug.ino b/Firmware/RTK_Surveyor/menuDebug.ino
index 4f1e091a5..582934901 100644
--- a/Firmware/RTK_Surveyor/menuDebug.ino
+++ b/Firmware/RTK_Surveyor/menuDebug.ino
@@ -38,6 +38,8 @@ void menuDebug()
     if (settings.enableResetDisplay == true) Serial.println(F("Enabled"));
     else Serial.println(F("Disabled"));
 
+    Serial.println(F("r) Force system reset"));
+
     Serial.println(F("x) Exit"));
 
     byte incoming = getByteChoice(menuTimeout); //Timeout after x seconds
@@ -105,6 +107,15 @@ void menuDebug()
     else if (incoming == '8')
     {
       settings.enableResetDisplay ^= 1;
+      if (settings.enableResetDisplay == true)
+      {
+        settings.resetCount = 0;
+        recordSystemSettings(); //Record to NVM
+      }
+    }
+    else if (incoming == 'r')
+    {
+      ESP.restart();
     }
     else if (incoming == 'x')
       break;

From b79c91d0bf097f905b8d0603737da683d8e57f04 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Fri, 17 Sep 2021 15:30:43 -0600
Subject: [PATCH 03/17] Test for online EEPROM before accessing.

---
 Firmware/RTK_Surveyor/NVM.ino | 48 +++++++++++++++++++++++++----------
 1 file changed, 35 insertions(+), 13 deletions(-)

diff --git a/Firmware/RTK_Surveyor/NVM.ino b/Firmware/RTK_Surveyor/NVM.ino
index e00a24bd0..c368caaa8 100644
--- a/Firmware/RTK_Surveyor/NVM.ino
+++ b/Firmware/RTK_Surveyor/NVM.ino
@@ -1,5 +1,11 @@
 void loadSettings()
 {
+  if (online.eeprom == false)
+  {
+    ESP_LOGD(TAG, "Error: EEPROM not online");
+    return;
+  }
+
   //First load any settings from NVM
   //After, we'll load settings from config file if available
   //We'll then re-record settings so that the settings from the file over-rides internal NVM settings
@@ -41,25 +47,44 @@ void loadSettings()
   recordSystemSettings();
 }
 
+//Load settings without recording 
+//Used at very first boot to test for resetCounter
+void loadSettingsPartial()
+{
+  if (online.eeprom == false)
+  {
+    ESP_LOGD(TAG, "Error: EEPROM not online");
+    return;
+  }
+
+  //Check to see if EEPROM is blank
+  uint32_t testRead = 0;
+  if (EEPROM.get(0, testRead) == 0xFFFFFFFF)
+    return; //EEPROM is blank, assume default settings
+  
+  EEPROM.get(0, settings); //Read current settings
+}
+
 //Record the current settings struct to EEPROM and then to config file
 void recordSystemSettings()
 {
   settings.sizeOfSettings = sizeof(settings);
   if (settings.sizeOfSettings > EEPROM_SIZE)
   {
-    displayError("EEPROM");
+    Serial.printf("Size of settings is %d bytes\n\r", sizeof(settings));
+    Serial.println(F("Increase the EEPROM footprint!"));
+    displayError("EEPROM"); //Hard freeze
+  }
 
-    while (1) //Hard freeze
-    {
-      Serial.printf("Size of settings is %d bytes\n\r", sizeof(settings));
-      Serial.println(F("Increase the EEPROM footprint!"));
-      delay(1000);
-    }
+  if (online.eeprom == true)
+  {
+    EEPROM.put(0, settings);
+    EEPROM.commit();
+    delay(1); //Give CPU time to pet WDT
   }
+  else
+    ESP_LOGD(TAG, "Error: EEPROM not online");
 
-  EEPROM.put(0, settings);
-  EEPROM.commit();
-  delay(1); //Give CPU time to pet WDT
   recordSystemSettingsToFile();
 }
 
@@ -142,7 +167,6 @@ void recordSystemSettingsToFile()
       settingsFile.println("enableSensorFusion=" + (String)settings.enableSensorFusion);
       settingsFile.println("autoIMUmountAlignment=" + (String)settings.autoIMUmountAlignment);
       settingsFile.println("enableResetDisplay=" + (String)settings.enableResetDisplay);
-      settingsFile.println("resetCount=" + (String)settings.resetCount);
 
       //Record constellation settings
       for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
@@ -411,8 +435,6 @@ bool parseLine(char* str) {
     settings.autoIMUmountAlignment = d;
   else if (strcmp(settingName, "enableResetDisplay") == 0)
     settings.enableResetDisplay = d;
-  else if (strcmp(settingName, "resetCount") == 0)
-    settings.resetCount = d;
 
   //Check for bulk settings (constellations and message rates)
   //Must be last on else list

From c616989a8671c07378c9e0e210a403309c3b904c Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Fri, 17 Sep 2021 16:26:38 -0600
Subject: [PATCH 04/17] Properly bring ubxMsg and ubxConstellation arrays into
 settings/NVM. Add ESF keys.

---
 Firmware/RTK_Surveyor/Begin.ino        |   2 +-
 Firmware/RTK_Surveyor/Form.ino         |  20 +-
 Firmware/RTK_Surveyor/NVM.ino          |  20 +-
 Firmware/RTK_Surveyor/Rover.ino        |   2 +-
 Firmware/RTK_Surveyor/System.ino       |   2 +-
 Firmware/RTK_Surveyor/menuGNSS.ino     |  22 +-
 Firmware/RTK_Surveyor/menuMessages.ino |  34 +--
 Firmware/RTK_Surveyor/menuTest.ino     |   2 +-
 Firmware/RTK_Surveyor/settings.h       | 273 +++++++++++--------------
 9 files changed, 170 insertions(+), 207 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino
index 0f8b12ba7..39edd7114 100644
--- a/Firmware/RTK_Surveyor/Begin.ino
+++ b/Firmware/RTK_Surveyor/Begin.ino
@@ -38,7 +38,7 @@ void beginBoard()
 
     //Bug in ZED-F9P v1.13 firmware causes RTK LED to not light when RTK Floating with SBAS on.
     //The following changes the POR default but will be overwritten by settings in NVM or settings file
-    ubxConstellations[1].enabled = false;
+    settings.ubxConstellations[1].enabled = false;
 
     strcpy(platformFilePrefix, "SFE_Surveyor");
     strcpy(platformPrefix, "Surveyor");
diff --git a/Firmware/RTK_Surveyor/Form.ino b/Firmware/RTK_Surveyor/Form.ino
index cb7c0037f..aa1baad17 100644
--- a/Firmware/RTK_Surveyor/Form.ino
+++ b/Firmware/RTK_Surveyor/Form.ino
@@ -243,13 +243,13 @@ void createSettingsString(char* settingsCSV)
   //GNSS Config
   stringRecord(settingsCSV, "measurementRateHz", 1000.0 / settings.measurementRate, 2); //2 = decimals to print
   stringRecord(settingsCSV, "dynamicModel", settings.dynamicModel);
-  stringRecord(settingsCSV, "ubxConstellationsGPS", ubxConstellations[0].enabled); //GPS
-  stringRecord(settingsCSV, "ubxConstellationsSBAS", ubxConstellations[1].enabled); //SBAS
-  stringRecord(settingsCSV, "ubxConstellationsGalileo", ubxConstellations[2].enabled); //Galileo
-  stringRecord(settingsCSV, "ubxConstellationsBeiDou", ubxConstellations[3].enabled); //BeiDou
-  stringRecord(settingsCSV, "ubxConstellationsGLONASS", ubxConstellations[5].enabled); //GLONASS
+  stringRecord(settingsCSV, "ubxConstellationsGPS", settings.ubxConstellations[0].enabled); //GPS
+  stringRecord(settingsCSV, "ubxConstellationsSBAS", settings.ubxConstellations[1].enabled); //SBAS
+  stringRecord(settingsCSV, "ubxConstellationsGalileo", settings.ubxConstellations[2].enabled); //Galileo
+  stringRecord(settingsCSV, "ubxConstellationsBeiDou", settings.ubxConstellations[3].enabled); //BeiDou
+  stringRecord(settingsCSV, "ubxConstellationsGLONASS", settings.ubxConstellations[5].enabled); //GLONASS
   for (int x = 0 ; x < MAX_UBX_MSG ; x++)
-    stringRecord(settingsCSV, ubxMessages[x].msgTextName, ubxMessages[x].msgRate);
+    stringRecord(settingsCSV, settings.ubxMessages[x].msgTextName, settings.ubxMessages[x].msgRate);
 
   //Base Config
   stringRecord(settingsCSV, "baseTypeSurveyIn", !settings.fixedBase);
@@ -394,11 +394,11 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
       for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
       {
         char tempString[50]; //ubxConstellationsSBAS
-        sprintf(tempString, "ubxConstellations%s", ubxConstellations[x].textName);
+        sprintf(tempString, "ubxConstellations%s", settings.ubxConstellations[x].textName);
 
         if (strcmp(settingName, tempString) == 0)
         {
-          ubxConstellations[x].enabled = settingValueBool;
+          settings.ubxConstellations[x].enabled = settingValueBool;
           knownSetting = true;
           break;
         }
@@ -410,9 +410,9 @@ void updateSettingWithValue(const char *settingName, const char* settingValueStr
     {
       for (int x = 0 ; x < MAX_UBX_MSG ; x++)
       {
-        if (strcmp(settingName, ubxMessages[x].msgTextName) == 0)
+        if (strcmp(settingName, settings.ubxMessages[x].msgTextName) == 0)
         {
-          ubxMessages[x].msgRate = settingValue;
+          settings.ubxMessages[x].msgRate = settingValue;
           knownSetting = true;
           break;
         }
diff --git a/Firmware/RTK_Surveyor/NVM.ino b/Firmware/RTK_Surveyor/NVM.ino
index c368caaa8..1783f880d 100644
--- a/Firmware/RTK_Surveyor/NVM.ino
+++ b/Firmware/RTK_Surveyor/NVM.ino
@@ -60,7 +60,10 @@ void loadSettingsPartial()
   //Check to see if EEPROM is blank
   uint32_t testRead = 0;
   if (EEPROM.get(0, testRead) == 0xFFFFFFFF)
+  {
+    ESP_LOGD(TAG, "EEPROM is blank");
     return; //EEPROM is blank, assume default settings
+  }
   
   EEPROM.get(0, settings); //Read current settings
 }
@@ -172,7 +175,7 @@ void recordSystemSettingsToFile()
       for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
       {
         char tempString[50]; //constellation.BeiDou=1
-        sprintf(tempString, "constellation.%s=%d", ubxConstellations[x].textName, ubxConstellations[x].enabled);
+        sprintf(tempString, "constellation.%s=%d", settings.ubxConstellations[x].textName, settings.ubxConstellations[x].enabled);
         settingsFile.println(tempString);
       }
 
@@ -180,7 +183,7 @@ void recordSystemSettingsToFile()
       for (int x = 0 ; x < MAX_UBX_MSG ; x++)
       {
         char tempString[50]; //message.nmea_dtm.msgRate=5
-        sprintf(tempString, "message.%s.msgRate=%d", ubxMessages[x].msgTextName, ubxMessages[x].msgRate);
+        sprintf(tempString, "message.%s.msgRate=%d", settings.ubxMessages[x].msgTextName, settings.ubxMessages[x].msgRate);
         settingsFile.println(tempString);
       }
 
@@ -448,11 +451,11 @@ bool parseLine(char* str) {
       for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
       {
         char tempString[50]; //constellation.GPS=1
-        sprintf(tempString, "constellation.%s", ubxConstellations[x].textName);
+        sprintf(tempString, "constellation.%s", settings.ubxConstellations[x].textName);
 
         if (strcmp(settingName, tempString) == 0)
         {
-          ubxConstellations[x].enabled = d;
+          settings.ubxConstellations[x].enabled = d;
           knownSetting = true;
           break;
         }
@@ -465,11 +468,11 @@ bool parseLine(char* str) {
       for (int x = 0 ; x < MAX_UBX_MSG ; x++)
       {
         char tempString[50]; //message.nmea_dtm.msgRate=5
-        sprintf(tempString, "message.%s.msgRate", ubxMessages[x].msgTextName);
+        sprintf(tempString, "message.%s.msgRate", settings.ubxMessages[x].msgTextName);
 
         if (strcmp(settingName, tempString) == 0)
         {
-          ubxMessages[x].msgRate = d;
+          settings.ubxMessages[x].msgRate = d;
           knownSetting = true;
           break;
         }
@@ -489,6 +492,11 @@ bool parseLine(char* str) {
 //ESP32 doesn't have erase command so we do it here
 void eepromErase()
 {
+  if(online.eeprom == false)
+  {
+    ESP_LOGD(TAG, "Error: EEPROM not online");
+    return;
+  }
   for (int i = 0 ; i < EEPROM_SIZE ; i++) {
     EEPROM.write(i, 0xFF); //Reset to all 1s
   }
diff --git a/Firmware/RTK_Surveyor/Rover.ino b/Firmware/RTK_Surveyor/Rover.ino
index 67fcf8640..2aaf3b1d6 100644
--- a/Firmware/RTK_Surveyor/Rover.ino
+++ b/Firmware/RTK_Surveyor/Rover.ino
@@ -35,7 +35,7 @@ bool configureUbloxModuleRover()
   }
 
   //Re-enable any RTCM msgs on UART1 the user has set within settings
-  response &= configureGNSSMessageRates(COM_PORT_UART1, ubxMessages); //Make sure the appropriate messages are enabled
+  response &= configureGNSSMessageRates(COM_PORT_UART1, settings.ubxMessages); //Make sure the appropriate messages are enabled
 
   if (response == false)
     Serial.println(F("Disable RTCM failed"));
diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index 5903fb8e1..230086ad8 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -218,7 +218,7 @@ bool configureUbloxModule()
 
   response &= configureConstellations(); //Enable the constellations the user has set
 
-  response &= configureGNSSMessageRates(COM_PORT_UART1, ubxMessages); //Make sure the appropriate messages are enabled
+  response &= configureGNSSMessageRates(COM_PORT_UART1, settings.ubxMessages); //Make sure the appropriate messages are enabled
 
   response &= i2cGNSS.setAutoPVT(true, false); //Tell the GPS to "send" each solution, but do not update stale data when accessed
   response &= i2cGNSS.setAutoHPPOSLLH(true, false); //Tell the GPS to "send" each high res solution, but do not update stale data when accessed
diff --git a/Firmware/RTK_Surveyor/menuGNSS.ino b/Firmware/RTK_Surveyor/menuGNSS.ino
index ce9624c21..d1935739c 100644
--- a/Firmware/RTK_Surveyor/menuGNSS.ino
+++ b/Firmware/RTK_Surveyor/menuGNSS.ino
@@ -148,8 +148,8 @@ void menuConstellations()
 
     for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
     {
-      Serial.printf("%d) Constellation %s: ", x + 1, ubxConstellations[x].textName);
-      if (ubxConstellations[x].enabled == true)
+      Serial.printf("%d) Constellation %s: ", x + 1, settings.ubxConstellations[x].textName);
+      if (settings.ubxConstellations[x].enabled == true)
         Serial.print("Enabled");
       else
         Serial.print("Disabled");
@@ -164,13 +164,13 @@ void menuConstellations()
     {
       incoming--; //Align choice to constallation array of 0 to 5
       
-      ubxConstellations[incoming].enabled ^= 1;
+      settings.ubxConstellations[incoming].enabled ^= 1;
 
       //3.10.6: To avoid cross-correlation issues, it is recommended that GPS and QZSS are always both enabled or both disabled.
       if (incoming == SFE_UBLOX_GNSS_ID_GPS || incoming == 4) //QZSS ID is 5 but array location is 4
       {
-        ubxConstellations[SFE_UBLOX_GNSS_ID_GPS].enabled = ubxConstellations[incoming].enabled; //GPS ID is 0 and array location is 0
-        ubxConstellations[4].enabled = ubxConstellations[incoming].enabled; //QZSS ID is 5 but array location is 4
+        settings.ubxConstellations[SFE_UBLOX_GNSS_ID_GPS].enabled = settings.ubxConstellations[incoming].enabled; //GPS ID is 0 and array location is 0
+        settings.ubxConstellations[4].enabled = settings.ubxConstellations[incoming].enabled; //QZSS ID is 5 but array location is 4
       }
     }
     else if (incoming == STATUS_PRESSED_X)
@@ -258,14 +258,14 @@ bool configureConstellations()
   for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
   {
     //Standard UBX protocol method takes ~533-783ms
-    uint8_t currentlyEnabled = getConstellation(ubxConstellations[x].gnssID); //Qeury the module for the current setting
-    if (currentlyEnabled != ubxConstellations[x].enabled)
-      response &= setConstellation(ubxConstellations[x].gnssID, ubxConstellations[x].enabled);
+    uint8_t currentlyEnabled = getConstellation(settings.ubxConstellations[x].gnssID); //Qeury the module for the current setting
+    if (currentlyEnabled != settings.ubxConstellations[x].enabled)
+      response &= setConstellation(settings.ubxConstellations[x].gnssID, settings.ubxConstellations[x].enabled);
 
     //Get/set val method takes ~642ms but does not work because we don't send additional sigCfg keys at same time
-    //    uint8_t currentlyEnabled = i2cGNSS.getVal8(ubxConstellations[x].configKey, VAL_LAYER_RAM, 1200);
-    //    if (currentlyEnabled != ubxConstellations[x].enabled)
-    //      response &= i2cGNSS.setVal(ubxConstellations[x].configKey, ubxConstellations[x].enabled);
+    //    uint8_t currentlyEnabled = i2cGNSS.getVal8(settings.ubxConstellations[x].configKey, VAL_LAYER_RAM, 1200);
+    //    if (currentlyEnabled != settings.ubxConstellations[x].enabled)
+    //      response &= i2cGNSS.setVal(settings.ubxConstellations[x].configKey, settings.ubxConstellations[x].enabled);
   }
   //long stopTime = millis();
 
diff --git a/Firmware/RTK_Surveyor/menuMessages.ino b/Firmware/RTK_Surveyor/menuMessages.ino
index f0b2d0f13..335fa52b0 100644
--- a/Firmware/RTK_Surveyor/menuMessages.ino
+++ b/Firmware/RTK_Surveyor/menuMessages.ino
@@ -108,7 +108,7 @@ void menuMessages()
       menuMessagesSubtype("TIM");
     else if (incoming == 7)
     {
-      setGNSSMessageRates(ubxMessages, 0); //Turn off all messages
+      setGNSSMessageRates(settings.ubxMessages, 0); //Turn off all messages
       setMessageRateByName("UBX_NMEA_GGA", 1);
       setMessageRateByName("UBX_NMEA_GSA", 1);
       setMessageRateByName("UBX_NMEA_GST", 1);
@@ -118,7 +118,7 @@ void menuMessages()
     }
     else if (incoming == 8)
     {
-      setGNSSMessageRates(ubxMessages, 0); //Turn off all messages
+      setGNSSMessageRates(settings.ubxMessages, 0); //Turn off all messages
       setMessageRateByName("UBX_NMEA_GGA", 1);
       setMessageRateByName("UBX_NMEA_GSA", 1);
       setMessageRateByName("UBX_NMEA_GST", 1);
@@ -131,12 +131,12 @@ void menuMessages()
     }
     else if (incoming == 9)
     {
-      setGNSSMessageRates(ubxMessages, 0); //Turn off all messages
+      setGNSSMessageRates(settings.ubxMessages, 0); //Turn off all messages
       Serial.println(F("All messages disabled"));
     }
     else if (incoming == 10)
     {
-      setGNSSMessageRates(ubxMessages, 1); //Turn on all messages to report once per fix
+      setGNSSMessageRates(settings.ubxMessages, 1); //Turn on all messages to report once per fix
       Serial.println(F("All messages enabled"));
     }
     else if (incoming == STATUS_PRESSED_X)
@@ -149,12 +149,12 @@ void menuMessages()
 
   while (Serial.available()) Serial.read(); //Empty buffer of any newline chars
 
-  bool response = configureGNSSMessageRates(COM_PORT_UART1, ubxMessages); //Make sure the appropriate messages are enabled
+  bool response = configureGNSSMessageRates(COM_PORT_UART1, settings.ubxMessages); //Make sure the appropriate messages are enabled
   if (response == false)
   {
     Serial.println(F("menuMessages: Failed to enable UART1 messages - Try 1"));
     //Try again
-    response = configureGNSSMessageRates(COM_PORT_UART1, ubxMessages); //Make sure the appropriate messages are enabled
+    response = configureGNSSMessageRates(COM_PORT_UART1, settings.ubxMessages); //Make sure the appropriate messages are enabled
     if (response == false)
       Serial.println(F("menuMessages: Failed to enable UART1 messages - Try 2"));
     else
@@ -182,10 +182,10 @@ void menuMessagesSubtype(const char* messageType)
     for (int x = 0 ; x < (endOfBlock - startOfBlock) ; x++)
     {
       //Check to see if this ZED platform supports this message
-      if (ubxMessages[x + startOfBlock].supported & zedModuleType)
+      if (settings.ubxMessages[x + startOfBlock].supported & zedModuleType)
       {
-        Serial.printf("%d) Message %s: ", x + 1, ubxMessages[x + startOfBlock].msgTextName);
-        Serial.println(ubxMessages[x + startOfBlock].msgRate);
+        Serial.printf("%d) Message %s: ", x + 1, settings.ubxMessages[x + startOfBlock].msgTextName);
+        Serial.println(settings.ubxMessages[x + startOfBlock].msgRate);
       }
     }
 
@@ -196,8 +196,8 @@ void menuMessagesSubtype(const char* messageType)
     if (incoming >= 1 && incoming <= (endOfBlock - startOfBlock))
     {
       //Check to see if this ZED platform supports this message
-      if (ubxMessages[(incoming - 1) + startOfBlock].supported & zedModuleType)
-        inputMessageRate(ubxMessages[(incoming - 1) + startOfBlock]);
+      if (settings.ubxMessages[(incoming - 1) + startOfBlock].supported & zedModuleType)
+        inputMessageRate(settings.ubxMessages[(incoming - 1) + startOfBlock]);
       else
         printUnknown(incoming);
     }
@@ -245,7 +245,7 @@ bool configureGNSSMessageRates(uint8_t portType, ubxMsg *localMessage)
   for (int x = 0 ; x < MAX_UBX_MSG ; x++)
   {
     //Check to see if this ZED platform supports this message
-    if (ubxMessages[x].supported & zedModuleType)
+    if (settings.ubxMessages[x].supported & zedModuleType)
       response &= configureMessageRate(portType, localMessage[x]);
   }
 
@@ -499,7 +499,7 @@ void setMessageOffsets(const char* messageType, int& startOfBlock, int& endOfBlo
   //Find the first occurrence
   for (startOfBlock = 0 ; startOfBlock < MAX_UBX_MSG ; startOfBlock++)
   {
-    if (strstr(ubxMessages[startOfBlock].msgTextName, messageNamePiece) != NULL) break;
+    if (strstr(settings.ubxMessages[startOfBlock].msgTextName, messageNamePiece) != NULL) break;
   }
   if (startOfBlock == MAX_UBX_MSG)
   {
@@ -512,7 +512,7 @@ void setMessageOffsets(const char* messageType, int& startOfBlock, int& endOfBlo
   //Find the last occurrence
   for (endOfBlock = startOfBlock + 1 ; endOfBlock < MAX_UBX_MSG ; endOfBlock++)
   {
-    if (strstr(ubxMessages[endOfBlock].msgTextName, messageNamePiece) == NULL) break;
+    if (strstr(settings.ubxMessages[endOfBlock].msgTextName, messageNamePiece) == NULL) break;
   }
 }
 
@@ -521,7 +521,7 @@ uint8_t getActiveMessageCount()
 {
   uint8_t count = 0;
   for (int x = 0 ; x < MAX_UBX_MSG ; x++)
-    if (ubxMessages[x].msgRate > 0) count++;
+    if (settings.ubxMessages[x].msgRate > 0) count++;
   return (count);
 }
 
@@ -530,9 +530,9 @@ bool setMessageRateByName(const char *msgName, uint8_t msgRate)
 {
   for (int x = 0 ; x < MAX_UBX_MSG ; x++)
   {
-    if (strcmp(ubxMessages[x].msgTextName, msgName) == 0)
+    if (strcmp(settings.ubxMessages[x].msgTextName, msgName) == 0)
     {
-      ubxMessages[x].msgRate = msgRate;
+      settings.ubxMessages[x].msgRate = msgRate;
       return (true);
     }
   }
diff --git a/Firmware/RTK_Surveyor/menuTest.ino b/Firmware/RTK_Surveyor/menuTest.ino
index a8844980b..2c68ce044 100644
--- a/Firmware/RTK_Surveyor/menuTest.ino
+++ b/Firmware/RTK_Surveyor/menuTest.ino
@@ -95,7 +95,7 @@ void menuTest()
     else if (incoming == 4)
     {
       //Send the current settings to USB
-      bool response = configureGNSSMessageRates(COM_PORT_USB, ubxMessages); //Make sure the appropriate messages are enabled
+      bool response = configureGNSSMessageRates(COM_PORT_USB, settings.ubxMessages); //Make sure the appropriate messages are enabled
       if (response == false)
         Serial.println(F("menuTest: Failed to enable USB messages"));
       else
diff --git a/Firmware/RTK_Surveyor/settings.h b/Firmware/RTK_Surveyor/settings.h
index 9c13b20b0..e45053678 100644
--- a/Firmware/RTK_Surveyor/settings.h
+++ b/Firmware/RTK_Surveyor/settings.h
@@ -108,17 +108,8 @@ typedef struct ubxConstellation
 
 //These are the allowable constellations to receive from and log (if enabled)
 //Tested with u-center v21.02
-#define MAX_CONSTELLATIONS (sizeof(ubxConstellations)/sizeof(ubxConstellation))
-ubxConstellation ubxConstellations[] =
-{
-  {UBLOX_CFG_SIGNAL_GPS_ENA, SFE_UBLOX_GNSS_ID_GPS, true, "GPS"},
-  {UBLOX_CFG_SIGNAL_SBAS_ENA, SFE_UBLOX_GNSS_ID_SBAS, true, "SBAS"},
-  {UBLOX_CFG_SIGNAL_GAL_ENA, SFE_UBLOX_GNSS_ID_GALILEO, true, "Galileo"},
-  {UBLOX_CFG_SIGNAL_BDS_ENA, SFE_UBLOX_GNSS_ID_BEIDOU, true, "BeiDou"},
-  //{UBLOX_CFG_SIGNAL_QZSS_ENA, SFE_UBLOX_GNSS_ID_IMES, false, "IMES"}, //Not yet supported? Config key does not exist?
-  {UBLOX_CFG_SIGNAL_QZSS_ENA, SFE_UBLOX_GNSS_ID_QZSS, true, "QZSS"},
-  {UBLOX_CFG_SIGNAL_GLO_ENA, SFE_UBLOX_GNSS_ID_GLONASS, true, "GLONASS"},
-};
+#define MAX_CONSTELLATIONS 6 //(sizeof(ubxConstellations)/sizeof(ubxConstellation))
+
 
 //Different ZED modules support different messages (F9P vs F9R vs F9T)
 //Create binary packed struct for different platforms
@@ -142,152 +133,7 @@ typedef struct ubxMsg
 
 //These are the allowable messages to broadcast and log (if enabled)
 //Tested with u-center v21.02
-#define MAX_UBX_MSG (sizeof(ubxMessages)/sizeof(ubxMsg))
-ubxMsg ubxMessages[] =
-{
-  {UBLOX_CFG_MSGOUT_NMEA_ID_DTM_UART1, UBX_NMEA_DTM, UBX_CLASS_NMEA, 0, "UBX_NMEA_DTM", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GBS_UART1, UBX_NMEA_GBS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GBS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1, UBX_NMEA_GGA, UBX_CLASS_NMEA, 1, "UBX_NMEA_GGA", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1, UBX_NMEA_GLL, UBX_CLASS_NMEA, 0, "UBX_NMEA_GLL", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GNS_UART1, UBX_NMEA_GNS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GNS", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GRS_UART1, UBX_NMEA_GRS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GRS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1, UBX_NMEA_GSA, UBX_CLASS_NMEA, 1, "UBX_NMEA_GSA", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GST_UART1, UBX_NMEA_GST, UBX_CLASS_NMEA, 1, "UBX_NMEA_GST", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_GSV_UART1, UBX_NMEA_GSV, UBX_CLASS_NMEA, 4, "UBX_NMEA_GSV", (PLATFORM_F9P | PLATFORM_F9R)}, //Default to 1 update every 4 fixes
-  {UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1, UBX_NMEA_RMC, UBX_CLASS_NMEA, 1, "UBX_NMEA_RMC", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_NMEA_ID_VLW_UART1, UBX_NMEA_VLW, UBX_CLASS_NMEA, 0, "UBX_NMEA_VLW", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1, UBX_NMEA_VTG, UBX_CLASS_NMEA, 0, "UBX_NMEA_VTG", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_UART1, UBX_NMEA_ZDA, UBX_CLASS_NMEA, 0, "UBX_NMEA_ZDA", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  //  uint8_t nmea_msb = 0; //Not supported by u-center
-  //  uint8_t nmea_gaq = 0; //Not supported by u-center
-  //  uint8_t nmea_gbq = 0; //Not supported by u-center
-  //  uint8_t nmea_glq = 0; //Not supported by u-center
-  //  uint8_t nmea_gnq = 0; //Not supported by u-center
-  //  uint8_t nmea_gpq = 0; //Not supported by u-center
-  //  uint8_t nmea_gqq = 0; //Not supported by u-center
-  //  uint8_t nmea_rlm = 0; //Not supported by u-center
-  //  uint8_t nmea_txt = 0; //Not supported by u-center
-  //  uint8_t nmea_ths = 0; //Not supported by ZED-F9P
-
-  {UBLOX_CFG_MSGOUT_UBX_NAV_ATT_UART1, UBX_NAV_ATT, UBX_CLASS_NAV, 0, "UBX_NAV_ATT", (PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_UART1, UBX_NAV_CLOCK, UBX_CLASS_NAV, 0, "UBX_NAV_CLOCK", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_DOP_UART1, UBX_NAV_DOP, UBX_CLASS_NAV, 0, "UBX_NAV_DOP", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_EOE_UART1, UBX_NAV_EOE, UBX_CLASS_NAV, 0, "UBX_NAV_EOE", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_UART1, UBX_NAV_GEOFENCE, UBX_CLASS_NAV, 0, "UBX_NAV_GEOFENCE", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART1, UBX_NAV_HPPOSECEF, UBX_CLASS_NAV, 0, "UBX_NAV_HPPOSECEF", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART1, UBX_NAV_HPPOSLLH, UBX_CLASS_NAV, 0, "UBX_NAV_HPPOSLLH", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_ODO_UART1, UBX_NAV_ODO, UBX_CLASS_NAV, 0, "UBX_NAV_ODO", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART1, UBX_NAV_ORB, UBX_CLASS_NAV, 0, "UBX_NAV_ORB", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART1, UBX_NAV_POSECEF, UBX_CLASS_NAV, 0, "UBX_NAV_POSECEF", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_UART1, UBX_NAV_POSLLH, UBX_CLASS_NAV, 0, "UBX_NAV_POSLLH", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_PVT_UART1, UBX_NAV_PVT, UBX_CLASS_NAV, 0, "UBX_NAV_PVT", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_UART1, UBX_NAV_RELPOSNED, UBX_CLASS_NAV, 0, "UBX_NAV_RELPOSNED", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_SAT_UART1, UBX_NAV_SAT, UBX_CLASS_NAV, 0, "UBX_NAV_SAT", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_SIG_UART1, UBX_NAV_SIG, UBX_CLASS_NAV, 0, "UBX_NAV_SIG", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_UART1, UBX_NAV_STATUS, UBX_CLASS_NAV, 0, "UBX_NAV_STATUS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_UART1, UBX_NAV_SVIN, UBX_CLASS_NAV, 0, "UBX_NAV_SVIN", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_UART1, UBX_NAV_TIMEBDS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEBDS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_UART1, UBX_NAV_TIMEGAL, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGAL", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_UART1, UBX_NAV_TIMEGLO, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGLO", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_UART1, UBX_NAV_TIMEGPS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGPS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_UART1, UBX_NAV_TIMELS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMELS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_UART1, UBX_NAV_TIMEUTC, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEUTC", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_UART1, UBX_NAV_VELECEF, UBX_CLASS_NAV, 0, "UBX_NAV_VELECEF", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_UART1, UBX_NAV_VELNED, UBX_CLASS_NAV, 0, "UBX_NAV_VELNED", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  //  uint8_t nav_aopstatus = 0; //Not supported by library or ZED-F9P
-  //  uint8_t nav_att = 0; //Not supported by ZED-F9P
-  //  uint8_t nav_cov = 0; //Not supported by library
-  //  uint8_t nav_resetodo = 0; //Not supported u-center 21.02
-  //  uint8_t nav_sbas = 0; //Not supported by library
-  //  uint8_t nav_slas = 0; //Not supported by library
-  //  uint8_t nav_timeqzss = 0; //Not supported in library
-  //  uint8_t nav_dgps = 0; //Not supported by ZED-F9P
-  //  uint8_t nav_eell = 0; //Not supported in library
-  //  uint8_t nav_ekfstatus = 0; //Not supported by ZED-F9P
-  //  uint8_t nav_nmi = 0; //Not supported by ZED-F9P or library
-  //  uint8_t nav_sol = 0; //Not supported by ZED-F9P or library
-  //  uint8_t nav_svinfo = 0; //Not supported by ZED-F9P or library
-
-  {UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART1, UBX_RXM_MEASX, UBX_CLASS_RXM, 0, "UBX_RXM_MEASX", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_UART1, UBX_RXM_RAWX, UBX_CLASS_RXM, 0, "UBX_RXM_RAWX", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_RXM_RLM_UART1, UBX_RXM_RLM, UBX_CLASS_RXM, 0, "UBX_RXM_RLM", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_UART1, UBX_RXM_RTCM, UBX_CLASS_RXM, 0, "UBX_RXM_RTCM", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_UART1, UBX_RXM_SFRBX, UBX_CLASS_RXM, 0, "UBX_RXM_SFRBX", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  //  uint8_t rxm_alm = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_eph = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_imes = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_pmp = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_raw = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_sfrb = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_spartn = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_svsi = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_tm = 0; //Not supported by library or ZED-F9P
-  //  uint8_t rxm_pmreq = 0; //Not supported by u-center
-
-  //  uint8_t hnr_att = 0; //Not supported by ZED-F9P
-  //  uint8_t hnr_ins = 0; //Not supported by ZED-F9P
-  //  uint8_t hnr_pvt = 0; //Not supported by ZED-F9P
-
-  {UBLOX_CFG_MSGOUT_UBX_MON_COMMS_UART1, UBX_MON_COMMS, UBX_CLASS_MON, 0, "UBX_MON_COMMS", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_HW2_UART1, UBX_MON_HW2, UBX_CLASS_MON, 0, "UBX_MON_HW2", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_HW3_UART1, UBX_MON_HW3, UBX_CLASS_MON, 0, "UBX_MON_HW3", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_HW_UART1, UBX_MON_HW, UBX_CLASS_MON, 0, "UBX_MON_HW", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_IO_UART1, UBX_MON_IO, UBX_CLASS_MON, 0, "UBX_MON_IO", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  {UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_UART1, UBX_MON_MSGPP, UBX_CLASS_MON, 0, "UBX_MON_MSGPP", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_RF_UART1, UBX_MON_RF, UBX_CLASS_MON, 0, "UBX_MON_RF", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_UART1, UBX_MON_RXBUF, UBX_CLASS_MON, 0, "UBX_MON_RXBUF", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_RXR_UART1, UBX_MON_RXR, UBX_CLASS_MON, 0, "UBX_MON_RXR", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_UART1, UBX_MON_TXBUF, UBX_CLASS_MON, 0, "UBX_MON_TXBUF", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  //  uint8_t mon_gnss = 0; //Not supported by u-center
-  //  uint8_t mon_patch = 0; //Not supported by u-center
-  //uint8_t mon_smgr = 0; //Not supported by library or ZED-F9P
-  //uint8_t mon_span = 0; //Not supported by library
-  //  uint8_t mon_ver = 0; //Not supported by u-center
-
-  {UBLOX_CFG_MSGOUT_UBX_TIM_TM2_UART1, UBX_TIM_TM2, UBX_CLASS_TIM, 0, "UBX_TIM_TM2", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART1, UBX_TIM_TP, UBX_CLASS_TIM, 0, "UBX_TIM_TP", (PLATFORM_F9P | PLATFORM_F9R)},
-  {UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_UART1, UBX_TIM_VRFY, UBX_CLASS_TIM, 0, "UBX_TIM_VRFY", (PLATFORM_F9P | PLATFORM_F9R)},
-
-  //  uint8_t tim_dosc = 0; //Not supported by library or ZED-F9P
-  //  uint8_t tim_fchg = 0; //Not supported by library or ZED-F9P
-  //  uint8_t tim_smeas = 0; //Not supported by library or ZED-F9P
-  //  uint8_t tim_svin = 0; //Not supported by library or ZED-F9P
-  //  uint8_t tim_tos = 0; //Not supported by library or ZED-F9P
-  //  uint8_t tim_vcocal = 0; //Not supported by library or ZED-F9P
-
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_UART1, UBX_RTCM_1005, UBX_RTCM_MSB, 0, "UBX_RTCM_1005", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_UART1, UBX_RTCM_1074, UBX_RTCM_MSB, 0, "UBX_RTCM_1074", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_UART1, UBX_RTCM_1077, UBX_RTCM_MSB, 0, "UBX_RTCM_1077", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_UART1, UBX_RTCM_1084, UBX_RTCM_MSB, 0, "UBX_RTCM_1084", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_UART1, UBX_RTCM_1087, UBX_RTCM_MSB, 0, "UBX_RTCM_1087", (PLATFORM_F9P)},
-
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_UART1, UBX_RTCM_1094, UBX_RTCM_MSB, 0, "UBX_RTCM_1094", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_UART1, UBX_RTCM_1097, UBX_RTCM_MSB, 0, "UBX_RTCM_1097", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_UART1, UBX_RTCM_1124, UBX_RTCM_MSB, 0, "UBX_RTCM_1124", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_UART1, UBX_RTCM_1127, UBX_RTCM_MSB, 0, "UBX_RTCM_1127", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_UART1, UBX_RTCM_1230, UBX_RTCM_MSB, 0, "UBX_RTCM_1230", (PLATFORM_F9P)},
-
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART1, UBX_RTCM_4072_0, UBX_RTCM_MSB, 0, "UBX_RTCM_4072_0", (PLATFORM_F9P)},
-  {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART1, UBX_RTCM_4072_1, UBX_RTCM_MSB, 0, "UBX_RTCM_4072_1", (PLATFORM_F9P)},
-
-  //Config keys exist but are not yet implemented in SparkFun u-blox library
-  {0x20910278, UBX_ESF_MEAS, UBX_CLASS_ESF, 0, "UBX_ESF_MEAS", (PLATFORM_F9R)}, 
-  {0x209102a0, UBX_ESF_RAW, UBX_CLASS_ESF, 0, "UBX_ESF_RAW", (PLATFORM_F9R)},
-  {0x20910106, UBX_ESF_STATUS, UBX_CLASS_ESF, 0, "UBX_ESF_STATUS", (PLATFORM_F9R)},
-  {0x20910110, UBX_ESF_ALG, UBX_CLASS_ESF, 0, "UBX_ESF_ALG", (PLATFORM_F9R)},
-  {0x20910115, UBX_ESF_INS, UBX_CLASS_ESF, 0, "UBX_ESF_INS", (PLATFORM_F9R)},
-};
+#define MAX_UBX_MSG (13 + 25 + 5 + 10 + 3 + 12 + 5) //(sizeof(ubxMessages)/sizeof(ubxMsg))
 
 //This is all the settings that can be set on RTK Surveyor. It's recorded to NVM and the config file.
 struct struct_settings {
@@ -319,7 +165,105 @@ struct struct_settings {
   float surveyInStartingAccuracy = 1.0; //Wait for 1m horizontal positional accuracy before starting survey in
   uint16_t measurementRate = 250; //Elapsed ms between GNSS measurements. 25ms to 65535ms. Default 4Hz.
   uint16_t navigationRate = 1; //Ratio between number of measurements and navigation solutions. Default 1 for 4Hz (with measurementRate).
-  ubxMsg ubxMessages; //Report rates for all known messages
+  ubxMsg ubxMessages[MAX_UBX_MSG] = //Report rates for all known messages
+  {
+    //NMEA
+    {UBLOX_CFG_MSGOUT_NMEA_ID_DTM_UART1, UBX_NMEA_DTM, UBX_CLASS_NMEA, 0, "UBX_NMEA_DTM", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GBS_UART1, UBX_NMEA_GBS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GBS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GGA_UART1, UBX_NMEA_GGA, UBX_CLASS_NMEA, 1, "UBX_NMEA_GGA", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GLL_UART1, UBX_NMEA_GLL, UBX_CLASS_NMEA, 0, "UBX_NMEA_GLL", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GNS_UART1, UBX_NMEA_GNS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GNS", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GRS_UART1, UBX_NMEA_GRS, UBX_CLASS_NMEA, 0, "UBX_NMEA_GRS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GSA_UART1, UBX_NMEA_GSA, UBX_CLASS_NMEA, 1, "UBX_NMEA_GSA", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GST_UART1, UBX_NMEA_GST, UBX_CLASS_NMEA, 1, "UBX_NMEA_GST", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_GSV_UART1, UBX_NMEA_GSV, UBX_CLASS_NMEA, 4, "UBX_NMEA_GSV", (PLATFORM_F9P | PLATFORM_F9R)}, //Default to 1 update every 4 fixes
+    {UBLOX_CFG_MSGOUT_NMEA_ID_RMC_UART1, UBX_NMEA_RMC, UBX_CLASS_NMEA, 1, "UBX_NMEA_RMC", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_NMEA_ID_VLW_UART1, UBX_NMEA_VLW, UBX_CLASS_NMEA, 0, "UBX_NMEA_VLW", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_VTG_UART1, UBX_NMEA_VTG, UBX_CLASS_NMEA, 0, "UBX_NMEA_VTG", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_NMEA_ID_ZDA_UART1, UBX_NMEA_ZDA, UBX_CLASS_NMEA, 0, "UBX_NMEA_ZDA", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    //NAV
+    {UBLOX_CFG_MSGOUT_UBX_NAV_ATT_UART1, UBX_NAV_ATT, UBX_CLASS_NAV, 0, "UBX_NAV_ATT", (PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_CLOCK_UART1, UBX_NAV_CLOCK, UBX_CLASS_NAV, 0, "UBX_NAV_CLOCK", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_DOP_UART1, UBX_NAV_DOP, UBX_CLASS_NAV, 0, "UBX_NAV_DOP", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_EOE_UART1, UBX_NAV_EOE, UBX_CLASS_NAV, 0, "UBX_NAV_EOE", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_GEOFENCE_UART1, UBX_NAV_GEOFENCE, UBX_CLASS_NAV, 0, "UBX_NAV_GEOFENCE", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSECEF_UART1, UBX_NAV_HPPOSECEF, UBX_CLASS_NAV, 0, "UBX_NAV_HPPOSECEF", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_HPPOSLLH_UART1, UBX_NAV_HPPOSLLH, UBX_CLASS_NAV, 0, "UBX_NAV_HPPOSLLH", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_ODO_UART1, UBX_NAV_ODO, UBX_CLASS_NAV, 0, "UBX_NAV_ODO", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_ORB_UART1, UBX_NAV_ORB, UBX_CLASS_NAV, 0, "UBX_NAV_ORB", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_POSECEF_UART1, UBX_NAV_POSECEF, UBX_CLASS_NAV, 0, "UBX_NAV_POSECEF", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_UBX_NAV_POSLLH_UART1, UBX_NAV_POSLLH, UBX_CLASS_NAV, 0, "UBX_NAV_POSLLH", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_PVT_UART1, UBX_NAV_PVT, UBX_CLASS_NAV, 0, "UBX_NAV_PVT", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_RELPOSNED_UART1, UBX_NAV_RELPOSNED, UBX_CLASS_NAV, 0, "UBX_NAV_RELPOSNED", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_SAT_UART1, UBX_NAV_SAT, UBX_CLASS_NAV, 0, "UBX_NAV_SAT", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_SIG_UART1, UBX_NAV_SIG, UBX_CLASS_NAV, 0, "UBX_NAV_SIG", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_UBX_NAV_STATUS_UART1, UBX_NAV_STATUS, UBX_CLASS_NAV, 0, "UBX_NAV_STATUS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_SVIN_UART1, UBX_NAV_SVIN, UBX_CLASS_NAV, 0, "UBX_NAV_SVIN", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEBDS_UART1, UBX_NAV_TIMEBDS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEBDS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGAL_UART1, UBX_NAV_TIMEGAL, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGAL", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGLO_UART1, UBX_NAV_TIMEGLO, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGLO", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEGPS_UART1, UBX_NAV_TIMEGPS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEGPS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMELS_UART1, UBX_NAV_TIMELS, UBX_CLASS_NAV, 0, "UBX_NAV_TIMELS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_TIMEUTC_UART1, UBX_NAV_TIMEUTC, UBX_CLASS_NAV, 0, "UBX_NAV_TIMEUTC", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_VELECEF_UART1, UBX_NAV_VELECEF, UBX_CLASS_NAV, 0, "UBX_NAV_VELECEF", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_NAV_VELNED_UART1, UBX_NAV_VELNED, UBX_CLASS_NAV, 0, "UBX_NAV_VELNED", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    //RXM
+    {UBLOX_CFG_MSGOUT_UBX_RXM_MEASX_UART1, UBX_RXM_MEASX, UBX_CLASS_RXM, 0, "UBX_RXM_MEASX", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_RXM_RAWX_UART1, UBX_RXM_RAWX, UBX_CLASS_RXM, 0, "UBX_RXM_RAWX", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_RXM_RLM_UART1, UBX_RXM_RLM, UBX_CLASS_RXM, 0, "UBX_RXM_RLM", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_RXM_RTCM_UART1, UBX_RXM_RTCM, UBX_CLASS_RXM, 0, "UBX_RXM_RTCM", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_RXM_SFRBX_UART1, UBX_RXM_SFRBX, UBX_CLASS_RXM, 0, "UBX_RXM_SFRBX", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    //MON
+    {UBLOX_CFG_MSGOUT_UBX_MON_COMMS_UART1, UBX_MON_COMMS, UBX_CLASS_MON, 0, "UBX_MON_COMMS", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_HW2_UART1, UBX_MON_HW2, UBX_CLASS_MON, 0, "UBX_MON_HW2", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_HW3_UART1, UBX_MON_HW3, UBX_CLASS_MON, 0, "UBX_MON_HW3", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_HW_UART1, UBX_MON_HW, UBX_CLASS_MON, 0, "UBX_MON_HW", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_IO_UART1, UBX_MON_IO, UBX_CLASS_MON, 0, "UBX_MON_IO", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    {UBLOX_CFG_MSGOUT_UBX_MON_MSGPP_UART1, UBX_MON_MSGPP, UBX_CLASS_MON, 0, "UBX_MON_MSGPP", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_RF_UART1, UBX_MON_RF, UBX_CLASS_MON, 0, "UBX_MON_RF", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_RXBUF_UART1, UBX_MON_RXBUF, UBX_CLASS_MON, 0, "UBX_MON_RXBUF", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_RXR_UART1, UBX_MON_RXR, UBX_CLASS_MON, 0, "UBX_MON_RXR", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_MON_TXBUF_UART1, UBX_MON_TXBUF, UBX_CLASS_MON, 0, "UBX_MON_TXBUF", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    //TIM
+    {UBLOX_CFG_MSGOUT_UBX_TIM_TM2_UART1, UBX_TIM_TM2, UBX_CLASS_TIM, 0, "UBX_TIM_TM2", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_TIM_TP_UART1, UBX_TIM_TP, UBX_CLASS_TIM, 0, "UBX_TIM_TP", (PLATFORM_F9P | PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_TIM_VRFY_UART1, UBX_TIM_VRFY, UBX_CLASS_TIM, 0, "UBX_TIM_VRFY", (PLATFORM_F9P | PLATFORM_F9R)},
+
+    //RTCM
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1005_UART1, UBX_RTCM_1005, UBX_RTCM_MSB, 0, "UBX_RTCM_1005", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1074_UART1, UBX_RTCM_1074, UBX_RTCM_MSB, 0, "UBX_RTCM_1074", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1077_UART1, UBX_RTCM_1077, UBX_RTCM_MSB, 0, "UBX_RTCM_1077", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1084_UART1, UBX_RTCM_1084, UBX_RTCM_MSB, 0, "UBX_RTCM_1084", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1087_UART1, UBX_RTCM_1087, UBX_RTCM_MSB, 0, "UBX_RTCM_1087", (PLATFORM_F9P)},
+
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1094_UART1, UBX_RTCM_1094, UBX_RTCM_MSB, 0, "UBX_RTCM_1094", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1097_UART1, UBX_RTCM_1097, UBX_RTCM_MSB, 0, "UBX_RTCM_1097", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1124_UART1, UBX_RTCM_1124, UBX_RTCM_MSB, 0, "UBX_RTCM_1124", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1127_UART1, UBX_RTCM_1127, UBX_RTCM_MSB, 0, "UBX_RTCM_1127", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE1230_UART1, UBX_RTCM_1230, UBX_RTCM_MSB, 0, "UBX_RTCM_1230", (PLATFORM_F9P)},
+
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_0_UART1, UBX_RTCM_4072_0, UBX_RTCM_MSB, 0, "UBX_RTCM_4072_0", (PLATFORM_F9P)},
+    {UBLOX_CFG_MSGOUT_RTCM_3X_TYPE4072_1_UART1, UBX_RTCM_4072_1, UBX_RTCM_MSB, 0, "UBX_RTCM_4072_1", (PLATFORM_F9P)},
+
+    //ESF
+    {UBLOX_CFG_MSGOUT_UBX_ESF_MEAS_UART1, UBX_ESF_MEAS, UBX_CLASS_ESF, 0, "UBX_ESF_MEAS", (PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_ESF_RAW_UART1, UBX_ESF_RAW, UBX_CLASS_ESF, 0, "UBX_ESF_RAW", (PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_ESF_STATUS_UART1, UBX_ESF_STATUS, UBX_CLASS_ESF, 0, "UBX_ESF_STATUS", (PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_ESF_ALG_UART1, UBX_ESF_ALG, UBX_CLASS_ESF, 0, "UBX_ESF_ALG", (PLATFORM_F9R)},
+    {UBLOX_CFG_MSGOUT_UBX_ESF_INS_UART1, UBX_ESF_INS, UBX_CLASS_ESF, 0, "UBX_ESF_INS", (PLATFORM_F9R)},
+  };
+
   bool enableI2Cdebug = false; //Turn on to display GNSS library debug messages
   bool enableHeapReport = false; //Turn on to display free heap
   bool enableTaskReports = false; //Turn on to display task high water marks
@@ -331,7 +275,18 @@ struct struct_settings {
   uint8_t dynamicModel = DYN_MODEL_PORTABLE;
   SystemState lastState = STATE_ROVER_NOT_STARTED; //For Express, start unit in last known state
   bool throttleDuringSPPCongestion = true;
-  ubxConstellation ubxConstellations; //Constellations monitored/used for fix
+  //Constellations monitored/used for fix
+  ubxConstellation ubxConstellations[MAX_CONSTELLATIONS] = 
+  {
+    {UBLOX_CFG_SIGNAL_GPS_ENA, SFE_UBLOX_GNSS_ID_GPS, true, "GPS"},
+    {UBLOX_CFG_SIGNAL_SBAS_ENA, SFE_UBLOX_GNSS_ID_SBAS, true, "SBAS"},
+    {UBLOX_CFG_SIGNAL_GAL_ENA, SFE_UBLOX_GNSS_ID_GALILEO, true, "Galileo"},
+    {UBLOX_CFG_SIGNAL_BDS_ENA, SFE_UBLOX_GNSS_ID_BEIDOU, true, "BeiDou"},
+    //{UBLOX_CFG_SIGNAL_QZSS_ENA, SFE_UBLOX_GNSS_ID_IMES, false, "IMES"}, //Not yet supported? Config key does not exist?
+    {UBLOX_CFG_SIGNAL_QZSS_ENA, SFE_UBLOX_GNSS_ID_QZSS, true, "QZSS"},
+    {UBLOX_CFG_SIGNAL_GLO_ENA, SFE_UBLOX_GNSS_ID_GLONASS, true, "GLONASS"},
+  };
+
   bool enableSensorFusion = false; //If IMU is available, avoid using it unless user specifically selects automotive
   bool autoIMUmountAlignment = true; //Allows unit to automatically establish device orientation in vehicle
   bool enableResetDisplay = false;

From b4aa69be72a66a42519af95c49113860742e1aa4 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Sat, 18 Sep 2021 15:08:40 -0600
Subject: [PATCH 05/17] Trigger factory reset if GNSS config data will cause
 system lock.

---
 Firmware/RTK_Surveyor/NVM.ino      |  2 ++
 Firmware/RTK_Surveyor/menuGNSS.ino | 10 +++++++++-
 2 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/Firmware/RTK_Surveyor/NVM.ino b/Firmware/RTK_Surveyor/NVM.ino
index 1783f880d..e54413272 100644
--- a/Firmware/RTK_Surveyor/NVM.ino
+++ b/Firmware/RTK_Surveyor/NVM.ino
@@ -86,7 +86,9 @@ void recordSystemSettings()
     delay(1); //Give CPU time to pet WDT
   }
   else
+  {
     ESP_LOGD(TAG, "Error: EEPROM not online");
+  }
 
   recordSystemSettingsToFile();
 }
diff --git a/Firmware/RTK_Surveyor/menuGNSS.ino b/Firmware/RTK_Surveyor/menuGNSS.ino
index d1935739c..785c0a135 100644
--- a/Firmware/RTK_Surveyor/menuGNSS.ino
+++ b/Firmware/RTK_Surveyor/menuGNSS.ino
@@ -163,7 +163,7 @@ void menuConstellations()
     if (incoming >= 1 && incoming <= MAX_CONSTELLATIONS)
     {
       incoming--; //Align choice to constallation array of 0 to 5
-      
+
       settings.ubxConstellations[incoming].enabled ^= 1;
 
       //3.10.6: To avoid cross-correlation issues, it is recommended that GPS and QZSS are always both enabled or both disabled.
@@ -254,6 +254,14 @@ bool configureConstellations()
 {
   bool response = true;
 
+  //If we have a corrupt constellation ID it can cause GNSS config to fail.
+  //Reset to factory defaults.
+  if (settings.ubxConstellations[0].gnssID == 255)
+  {
+    ESP_LOGD(TAG, "Constellation ID corrupt");
+    factoryReset();
+  }
+
   //long startTime = millis();
   for (int x = 0 ; x < MAX_CONSTELLATIONS ; x++)
   {

From bf0fc9cd62d57ee3437c64567a945d66a45aaf63 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Sat, 18 Sep 2021 20:08:45 -0600
Subject: [PATCH 06/17] Disable NMEA (and RTCM3) over I2C to decrease reset
 occurance.

NMEA at 4Hz including GxGSV increases the number of bytes to the ESP32. The ZED-F9x is known to have issues creating bad I2C traffic. These bad transmissions lead to possible ESP32 un-recoverable errors and resets. By decreasing I2C traffic we reduce the possibility of a reset.
---
 Firmware/RTK_Surveyor/System.ino | 30 ++++++++++++++----------------
 1 file changed, 14 insertions(+), 16 deletions(-)

diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index 230086ad8..7d21c4e58 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -177,44 +177,42 @@ bool configureUbloxModule()
   //but the phone can also provide RTCM data and a user may want to configure the ZED over Bluetooth.
   //So let's be sure to enable UBX+NMEA+RTCM on the input
   getPortSettings(COM_PORT_UART1); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3) || settingPayload[INPUT_SETTING] != (COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3))
-  {
+  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3))
     response &= i2cGNSS.setPortOutput(COM_PORT_UART1, COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3); //Set the UART1 to output UBX+NMEA+RTCM
+
+  if (settingPayload[INPUT_SETTING] != (COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3))
     response &= i2cGNSS.setPortInput(COM_PORT_UART1, COM_TYPE_NMEA | COM_TYPE_UBX | COM_TYPE_RTCM3); //Set the UART1 to input UBX+NMEA+RTCM
-  }
 
   //Disable SPI port - This is just to remove some overhead by ZED
   getPortSettings(COM_PORT_SPI); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != 0 || settingPayload[INPUT_SETTING] != 0)
-  {
+  if (settingPayload[OUTPUT_SETTING] != 0)
     response &= i2cGNSS.setPortOutput(COM_PORT_SPI, 0); //Disable all protocols
+  if (settingPayload[INPUT_SETTING] != 0)
     response &= i2cGNSS.setPortInput(COM_PORT_SPI, 0); //Disable all protocols
-  }
 
   getPortSettings(COM_PORT_UART2); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != COM_TYPE_RTCM3 || settingPayload[INPUT_SETTING] != COM_TYPE_RTCM3)
-  {
+  if (settingPayload[OUTPUT_SETTING] != COM_TYPE_RTCM3)
     response &= i2cGNSS.setPortOutput(COM_PORT_UART2, COM_TYPE_RTCM3); //Set the UART2 to output RTCM (in case this device goes into base mode)
+  if (settingPayload[INPUT_SETTING] != COM_TYPE_RTCM3)
     response &= i2cGNSS.setPortInput(COM_PORT_UART2, COM_TYPE_RTCM3); //Set the UART2 to input RTCM
-  }
 
   //Turn on RTCM over I2C port so that we can harvest RTCM over I2C and send out over WiFi
   //This is easier than parsing over UART because the library handles the frame detection
   getPortSettings(COM_PORT_I2C); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_RTCM3) || settingPayload[INPUT_SETTING] != COM_TYPE_UBX)
-  {
-    response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Set the I2C port to output UBX (config), NMEA (logging), and RTCM3 (casting)
+  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX))
+    response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX); //Set the I2C port to output UBX (config)
+  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+I2C+ RTCM3 - Set the I2C port to output UBX (config), and RTCM3 (casting)
+
+  if (settingPayload[INPUT_SETTING] != COM_TYPE_UBX)
     response &= i2cGNSS.setPortInput(COM_PORT_I2C, COM_TYPE_UBX); //Set the I2C port to input UBX only
-  }
 
   //The USB port on the ZED may be used for RTCM to/from the computer (as an NTRIP caster or client)
   //So let's be sure all protocols are on for the USB port
   getPortSettings(COM_PORT_USB); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3) || settingPayload[INPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3))
-  {
+  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3))
     response &= i2cGNSS.setPortOutput(COM_PORT_USB, (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3)); //Set the USB port to everything
+  if (settingPayload[INPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3))
     response &= i2cGNSS.setPortInput(COM_PORT_USB, (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3)); //Set the USB port to everything
-  }
 
   response &= configureConstellations(); //Enable the constellations the user has set
 

From 43ba79e7c07fcfa86f0f1a1870c3fad0ce154f5a Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Sat, 18 Sep 2021 20:26:10 -0600
Subject: [PATCH 07/17] Enable RTCM over I2C only in base mode.

---
 Firmware/RTK_Surveyor/Base.ino   | 9 +++++++++
 Firmware/RTK_Surveyor/Rover.ino  | 8 ++++++++
 Firmware/RTK_Surveyor/System.ino | 7 -------
 3 files changed, 17 insertions(+), 7 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Base.ino b/Firmware/RTK_Surveyor/Base.ino
index 2328e4b33..b423166c0 100644
--- a/Firmware/RTK_Surveyor/Base.ino
+++ b/Firmware/RTK_Surveyor/Base.ino
@@ -34,6 +34,15 @@ bool configureUbloxModuleBase()
     }
   }
 
+#define OUTPUT_SETTING 14
+
+  //Turn on RTCM so that we can harvest RTCM over I2C and send out over WiFi
+  //This is easier than parsing over UART because the library handles the frame detection
+  getPortSettings(COM_PORT_I2C); //Load the settingPayload with this port's settings
+  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3))
+    response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3); //Set the I2C port to output UBX (config), and RTCM3 (casting)
+  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+NMEA+RTCM3 - 
+
   //In base mode the Surveyor should output RTCM over UART2 and I2C ports:
   //(Primary) UART2 in case the Surveyor is connected via radio to rover
   //(Optional) I2C in case user wants base to connect to WiFi and NTRIP Serve to Caster
diff --git a/Firmware/RTK_Surveyor/Rover.ino b/Firmware/RTK_Surveyor/Rover.ino
index 2aaf3b1d6..96e012187 100644
--- a/Firmware/RTK_Surveyor/Rover.ino
+++ b/Firmware/RTK_Surveyor/Rover.ino
@@ -24,6 +24,14 @@ bool configureUbloxModuleRover()
       Serial.println(F("setDynamicModel failed"));
   }
 
+#define OUTPUT_SETTING 14
+
+  //Turn off all traffic except UBX to reduce I2C bus errors and ESP32 resets as much as possible
+  getPortSettings(COM_PORT_I2C); //Load the settingPayload with this port's settings
+  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX))
+    response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX); //Set the I2C port to output UBX (config)
+  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+I2C+ RTCM3 - Set the I2C port to output UBX (config), and RTCM3 (casting)
+
   //RTCM is only available on ZED-F9P modules
   if (zedModuleType == PLATFORM_F9P)
   {
diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index 7d21c4e58..36ef923a5 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -196,13 +196,6 @@ bool configureUbloxModule()
   if (settingPayload[INPUT_SETTING] != COM_TYPE_RTCM3)
     response &= i2cGNSS.setPortInput(COM_PORT_UART2, COM_TYPE_RTCM3); //Set the UART2 to input RTCM
 
-  //Turn on RTCM over I2C port so that we can harvest RTCM over I2C and send out over WiFi
-  //This is easier than parsing over UART because the library handles the frame detection
-  getPortSettings(COM_PORT_I2C); //Load the settingPayload with this port's settings
-  if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX))
-    response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX); //Set the I2C port to output UBX (config)
-  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+I2C+ RTCM3 - Set the I2C port to output UBX (config), and RTCM3 (casting)
-
   if (settingPayload[INPUT_SETTING] != COM_TYPE_UBX)
     response &= i2cGNSS.setPortInput(COM_PORT_I2C, COM_TYPE_UBX); //Set the I2C port to input UBX only
 

From 91f426f5036ef704dba3fb1eef33965aa444b85f Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Sat, 18 Sep 2021 20:59:06 -0600
Subject: [PATCH 08/17] Non-impact comment and function cleanup

---
 Firmware/RTK_Surveyor/Begin.ino        |  7 +++++--
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 19 ++++++-------------
 2 files changed, 11 insertions(+), 15 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino
index 39edd7114..c763f2eec 100644
--- a/Firmware/RTK_Surveyor/Begin.ino
+++ b/Firmware/RTK_Surveyor/Begin.ino
@@ -207,6 +207,9 @@ void beginSD()
     }
 
     online.microSD = true;
+
+    Serial.println(F("microSD online"));
+    scanForFirmware(); //See if SD card contains new firmware that should be loaded at startup
   }
   else
   {
@@ -225,7 +228,7 @@ void beginUART2()
       "UARTStart", //Just for humans
       2000, //Stack Size
       NULL, //Task input parameter
-      0, // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
+      0, // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest
       &pinUART2TaskHandle, //Task handle
       0); //Core where task should run, 0=core, 1=Arduino
 
@@ -472,6 +475,6 @@ void beginSystemState()
       "BtnCheck", //Just for humans
       buttonTaskStackSize, //Stack Size
       NULL, //Task input parameter
-      ButtonCheckTaskPriority, //Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
+      ButtonCheckTaskPriority,
       &ButtonCheckTaskHandle); //Task handle
 }
diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index f4c397392..3249b8f02 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -43,7 +43,7 @@ const int FIRMWARE_VERSION_MINOR = 6;
 
 #define COMPILE_WIFI //Comment out to remove all WiFi functionality
 #define COMPILE_BT //Comment out to disable all Bluetooth
-//#define ENABLE_DEVELOPER //Uncomment this line to enable special developer modes (don't check power button at startup)
+#define ENABLE_DEVELOPER //Uncomment this line to enable special developer modes (don't check power button at startup)
 
 //Define the RTK board identifier:
 //  This is an int which is unique to this variant of the RTK Surveyor hardware which allows us
@@ -217,7 +217,7 @@ uint8_t rBuffer[SERIAL_SIZE_RX]; //Buffer for reading from F9P to SPP
 uint8_t wBuffer[SERIAL_SIZE_RX]; //Buffer for writing from incoming SPP to F9P
 TaskHandle_t F9PSerialReadTaskHandle = NULL; //Store handles so that we can kill them if user goes into WiFi NTRIP Server mode
 TaskHandle_t F9PSerialWriteTaskHandle = NULL; //Store handles so that we can kill them if user goes into WiFi NTRIP Server mode
-const uint8_t F9PSerialWriteTaskPriority = 1; //3 being the highest, and 0 being the lowest.
+const uint8_t F9PSerialWriteTaskPriority = 1; //3 being the highest, and 0 being the lowest
 const uint8_t F9PSerialReadTaskPriority = 1;
 
 TaskHandle_t pinUART2TaskHandle = NULL; //Dummy task to start UART2 on core 0.
@@ -271,7 +271,7 @@ Button *setupBtn = NULL; //We can't instantiate the buttons here because we don'
 Button *powerBtn = NULL;
 
 TaskHandle_t ButtonCheckTaskHandle = NULL;
-const uint8_t ButtonCheckTaskPriority = 1; //3 being the highest, and 0 being the lowest.
+const uint8_t ButtonCheckTaskPriority = 1; //3 being the highest, and 0 being the lowest
 const int buttonTaskStackSize = 2000;
 
 const int shutDownButtonTime = 2000; //ms press and hold before shutdown
@@ -356,12 +356,12 @@ void setup()
   Serial.begin(115200); //UART0 for programming and debugging
 
   Wire.begin(); //Start I2C on core 1
-  //Wire.setClock(400000);
-  Wire.setClock(100000);
+  Wire.setClock(100000); //Confirm we are at 100kHz
 
   beginGNSS(); //Connect to GNSS
 
   beginEEPROM(); //Start EEPROM and SD for settings
+  //eepromErase(); //Must be before first use of EEPROM. Currently in beginBoard().
 
   beginBoard(); //Determine what hardware platform we are running on
 
@@ -369,21 +369,14 @@ void setup()
 
   beginLEDs(); //LED and PWM setup
 
-  //eepromErase();
-
   beginSD(); //Test if SD is present
-  if (online.microSD == true)
-  {
-    Serial.println(F("microSD online"));
-    scanForFirmware(); //See if SD card contains new firmware that should be loaded at startup
-  }
 
   loadSettings(); //Attempt to load settings after SD is started so we can read the settings file if available
 
   beginUART2(); //Start UART2 on core 0, used to receive serial from ZED and pass out over SPP
 
   beginFuelGauge(); //Configure battery fuel guage monitor
-  checkBatteryLevels(); //Force display so you see battery level immediately at power on
+  checkBatteryLevels(); //Force check so you see battery level immediately at power on
 
   configureGNSS(); //Configure ZED module
 

From 7a9c65488c00d109177f0713976d1ec9a4bb860b Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 09:26:08 -0600
Subject: [PATCH 09/17] Disable NMEA sentences on I2C, UART, and SPI port. Add
 more checks for SD presence before checking semaphore.

NMEA is already turned off on I2C port in general, but turn off specific sentences to try to reduce ZED-F9x module processing.
---
 Firmware/RTK_Surveyor/Form.ino     |  6 ++++++
 Firmware/RTK_Surveyor/System.ino   | 14 ++++++++++----
 Firmware/RTK_Surveyor/menuMain.ino | 15 +++++++++------
 3 files changed, 25 insertions(+), 10 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Form.ino b/Firmware/RTK_Surveyor/Form.ino
index aa1baad17..ffb158441 100644
--- a/Firmware/RTK_Surveyor/Form.ino
+++ b/Firmware/RTK_Surveyor/Form.ino
@@ -142,6 +142,12 @@ void startConfigAP()
 #ifdef COMPILE_WIFI
 static void handleFirmwareFileUpload(AsyncWebServerRequest *request, String fileName, size_t index, uint8_t *data, size_t len, bool final)
 {
+  if(online.microSD == false)
+  {
+    Serial.println(F("No SD card available"));
+    return;
+  }
+  
   //Attempt to write to file system. This avoids collisions with file writing in F9PSerialReadTask()
   if (xSemaphoreTake(xFATSemaphore, fatSemaphore_longWait_ms) != pdPASS) {
     Serial.println(F("Failed to get file system lock on firmware file"));
diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index 36ef923a5..f3330f474 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -211,13 +211,15 @@ bool configureUbloxModule()
 
   response &= configureGNSSMessageRates(COM_PORT_UART1, settings.ubxMessages); //Make sure the appropriate messages are enabled
 
+  response &= disableNMEASentences(COM_PORT_I2C); //Disable NMEA messages on all but UART1
+  response &= disableNMEASentences(COM_PORT_UART2);
+  response &= disableNMEASentences(COM_PORT_SPI);
+
   response &= i2cGNSS.setAutoPVT(true, false); //Tell the GPS to "send" each solution, but do not update stale data when accessed
   response &= i2cGNSS.setAutoHPPOSLLH(true, false); //Tell the GPS to "send" each high res solution, but do not update stale data when accessed
 
   if (zedModuleType == PLATFORM_F9R)
-  {
     response &= i2cGNSS.setAutoESFSTATUS(true, false); //Tell the GPS to "send" each ESF Status, but do not update stale data when accessed
-  }
 
   if (getSerialRate(COM_PORT_UART1) != settings.dataPortBaud)
   {
@@ -248,8 +250,6 @@ bool configureUbloxModule()
   return (response);
 }
 
-
-
 //Disable all the NMEA sentences on a given com port
 bool disableNMEASentences(uint8_t portType)
 {
@@ -597,6 +597,12 @@ bool createTestFile()
   SdFile testFile;
   char testFileName[40] = "testfile.txt";
 
+  if(online.microSD == false)
+  {
+    Serial.println(F("SD card offline"));
+    return(false);
+  }
+
   //Attempt to write to file system. This avoids collisions with file writing from other functions like recordSystemSettingsToFile() and F9PSerialReadTask()
   if (xSemaphoreTake(xFATSemaphore, fatSemaphore_shortWait_ms) == pdPASS)
   {
diff --git a/Firmware/RTK_Surveyor/menuMain.ino b/Firmware/RTK_Surveyor/menuMain.ino
index ed7ef8d60..9ce21bc4b 100644
--- a/Firmware/RTK_Surveyor/menuMain.ino
+++ b/Firmware/RTK_Surveyor/menuMain.ino
@@ -107,17 +107,20 @@ void factoryReset()
   eepromErase();
 
   //Assemble settings file name
-  char settingsFileName[40]; //SFE_Surveyor_Settings.txt
+  char settingsFileName[40]; //SFE_Express_Plus_Settings.txt
   strcpy(settingsFileName, platformFilePrefix);
   strcat(settingsFileName, "_Settings.txt");
 
   //Attempt to write to file system. This avoids collisions with file writing from other functions like recordSystemSettingsToFile() and F9PSerialReadTask()
-  if (xSemaphoreTake(xFATSemaphore, fatSemaphore_longWait_ms) == pdPASS)
+  if (settings.enableSD && online.microSD)
   {
-    if (sd.exists(settingsFileName))
-      sd.remove(settingsFileName);
-    xSemaphoreGive(xFATSemaphore);
-  } //End xFATSemaphore
+    if (xSemaphoreTake(xFATSemaphore, fatSemaphore_longWait_ms) == pdPASS)
+    {
+      if (sd.exists(settingsFileName))
+        sd.remove(settingsFileName);
+      xSemaphoreGive(xFATSemaphore);
+    } //End xFATSemaphore
+  }
 
   i2cGNSS.factoryReset(); //Reset everything: baud rate, I2C address, update rate, everything.
 

From 62d689a5e539b9eab28c9850dee2c6c07974d61b Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 10:48:41 -0600
Subject: [PATCH 10/17] Upgrade to ESP32 core v2.0.0. Bring in new SPP library
 changes.

---
 .../src/BluetoothSerial/BTAddress.cpp         |  96 ++++++++++
 .../src/BluetoothSerial/BTAddress.h           |  36 ++++
 .../src/BluetoothSerial/BTAdvertisedDevice.h  |  65 +++++++
 .../BluetoothSerial/BTAdvertisedDeviceSet.cpp |  78 ++++++++
 .../RTK_Surveyor/src/BluetoothSerial/BTScan.h |  42 +++++
 .../src/BluetoothSerial/BTScanResultsSet.cpp  |  95 ++++++++++
 .../src/BluetoothSerial/BluetoothSerial.cpp   | 169 ++++++++++++++++--
 .../src/BluetoothSerial/BluetoothSerial.h     |  19 +-
 8 files changed, 580 insertions(+), 20 deletions(-)
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
 create mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp

diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
new file mode 100644
index 000000000..1ed416f2b
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
@@ -0,0 +1,96 @@
+/*
+ * BTAddress.cpp
+ *
+ *  Created on: Jul 2, 2017
+ *      Author: kolban
+ *  Ported  on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+#include "BTAddress.h"
+#include <string>
+#include <sstream>
+#include <iomanip>
+#include <string.h>
+#include <stdio.h>
+#include <malloc.h>
+#ifdef ARDUINO_ARCH_ESP32
+#include "esp32-hal-log.h"
+#endif
+
+
+/**
+ * @brief Create an address from the native ESP32 representation.
+ * @param [in] address The native representation.
+ */
+BTAddress::BTAddress(esp_bd_addr_t address) {
+	memcpy(m_address, address, ESP_BD_ADDR_LEN);
+} // BTAddress
+
+
+/**
+ * @brief Create an address from a hex string
+ *
+ * A hex string is of the format:
+ * ```
+ * 00:00:00:00:00:00
+ * ```
+ * which is 17 characters in length.
+ *
+ * @param [in] stringAddress The hex representation of the address.
+ */
+BTAddress::BTAddress(std::string stringAddress) {
+	if (stringAddress.length() != 17) return;
+
+	int data[6];
+	sscanf(stringAddress.c_str(), "%x:%x:%x:%x:%x:%x", &data[0], &data[1], &data[2], &data[3], &data[4], &data[5]);
+	m_address[0] = (uint8_t) data[0];
+	m_address[1] = (uint8_t) data[1];
+	m_address[2] = (uint8_t) data[2];
+	m_address[3] = (uint8_t) data[3];
+	m_address[4] = (uint8_t) data[4];
+	m_address[5] = (uint8_t) data[5];
+} // BTAddress
+
+
+/**
+ * @brief Determine if this address equals another.
+ * @param [in] otherAddress The other address to compare against.
+ * @return True if the addresses are equal.
+ */
+bool BTAddress::equals(BTAddress otherAddress) {
+	return memcmp(otherAddress.getNative(), m_address, 6) == 0;
+} // equals
+
+
+/**
+ * @brief Return the native representation of the address.
+ * @return The native representation of the address.
+ */
+esp_bd_addr_t *BTAddress::getNative() {
+	return &m_address;
+} // getNative
+
+
+/**
+ * @brief Convert a BT address to a string.
+ *
+ * A string representation of an address is in the format:
+ *
+ * ```
+ * xx:xx:xx:xx:xx:xx
+ * ```
+ *
+ * @return The string representation of the address.
+ */
+std::string BTAddress::toString() {
+	auto size = 18;
+	char *res = (char*)malloc(size);
+	snprintf(res, size, "%02x:%02x:%02x:%02x:%02x:%02x", m_address[0], m_address[1], m_address[2], m_address[3], m_address[4], m_address[5]);
+	std::string ret(res);
+	free(res);
+	return ret;
+} // toString
+#endif
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
new file mode 100644
index 000000000..e23d34593
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
@@ -0,0 +1,36 @@
+/*
+ * BTAddress.h
+ *
+ *  Created on: Jul 2, 2017
+ *      Author: kolban
+ *  Ported  on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+
+#ifndef COMPONENTS_CPP_UTILS_BTADDRESS_H_
+#define COMPONENTS_CPP_UTILS_BTADDRESS_H_
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+#include <esp_gap_bt_api.h> // ESP32 BT
+#include <string>
+
+
+/**
+ * @brief A %BT device address.
+ *
+ * Every %BT device has a unique address which can be used to identify it and form connections.
+ */
+class BTAddress {
+public:
+	BTAddress(esp_bd_addr_t address);
+	BTAddress(std::string stringAddress);
+	bool           equals(BTAddress otherAddress);
+	esp_bd_addr_t* getNative();
+	std::string    toString();
+
+private:
+	esp_bd_addr_t m_address;
+};
+
+#endif /* CONFIG_BT_ENABLED */
+#endif /* COMPONENTS_CPP_UTILS_BTADDRESS_H_ */
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
new file mode 100644
index 000000000..07e93622e
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
@@ -0,0 +1,65 @@
+/*
+ * BTAdvertisedDevice.h
+ *
+ *  Created on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+
+#ifndef __BTADVERTISEDDEVICE_H__
+#define __BTADVERTISEDDEVICE_H__
+
+#include "BTAddress.h"
+
+
+class BTAdvertisedDevice {
+public:
+    virtual ~BTAdvertisedDevice() = default;
+
+    virtual BTAddress   getAddress();
+    virtual uint32_t    getCOD();
+    virtual std::string getName();
+    virtual int8_t      getRSSI();
+
+
+    virtual bool        haveCOD();
+    virtual bool        haveName();
+    virtual bool        haveRSSI();
+
+    virtual std::string toString();
+};
+
+class BTAdvertisedDeviceSet : public virtual BTAdvertisedDevice {
+public:
+    BTAdvertisedDeviceSet();
+    //~BTAdvertisedDeviceSet() = default;
+    
+
+    BTAddress   getAddress();
+    uint32_t    getCOD();
+    std::string getName();
+    int8_t      getRSSI();
+
+
+    bool        haveCOD();
+    bool        haveName();
+    bool        haveRSSI();
+
+    std::string toString();
+
+    void setAddress(BTAddress address);
+    void setCOD(uint32_t cod);
+    void setName(std::string name);
+    void setRSSI(int8_t rssi);
+
+    bool m_haveCOD;
+    bool m_haveName;
+    bool m_haveRSSI;
+
+
+    BTAddress   m_address = BTAddress((uint8_t*)"\0\0\0\0\0\0");
+    uint32_t    m_cod;
+    std::string m_name;
+    int8_t      m_rssi;
+};
+
+#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
new file mode 100644
index 000000000..78df93eab
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
@@ -0,0 +1,78 @@
+/*
+ * BTAdvertisedDeviceSet.cpp
+ *
+ *  Created on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+//#include <map>
+
+#include "BTAdvertisedDevice.h"
+//#include "BTScan.h"
+
+
+BTAdvertisedDeviceSet::BTAdvertisedDeviceSet() {
+	m_cod		       = 0;
+	m_name             = "";
+	m_rssi             = 0;
+
+	m_haveCOD	       = false;
+	m_haveName         = false;
+	m_haveRSSI         = false;
+} // BTAdvertisedDeviceSet
+
+BTAddress   BTAdvertisedDeviceSet::getAddress()    { return m_address; }
+uint32_t    BTAdvertisedDeviceSet::getCOD()        { return m_cod; }
+std::string BTAdvertisedDeviceSet::getName()       { return m_name; }
+int8_t      BTAdvertisedDeviceSet::getRSSI()	    { return m_rssi; }
+
+
+bool        BTAdvertisedDeviceSet::haveCOD()   { return m_haveCOD; }
+bool        BTAdvertisedDeviceSet::haveName()  { return m_haveName; }
+bool        BTAdvertisedDeviceSet::haveRSSI()  { return m_haveRSSI; }
+
+/**
+ * @brief Create a string representation of this device.
+ * @return A string representation of this device.
+ */
+std::string BTAdvertisedDeviceSet::toString() {
+	std::string res = "Name: " + getName() + ", Address: " + getAddress().toString();
+	if (haveCOD()) {
+		char val[6];
+		snprintf(val, sizeof(val), "%d", getCOD());
+		res += ", cod: ";
+		res += val;
+	}
+	if (haveRSSI()) {
+		char val[6];
+		snprintf(val, sizeof(val), "%d", (int8_t)getRSSI());
+		res += ", rssi: ";
+		res += val;
+	}
+	return res;
+} // toString
+
+
+void BTAdvertisedDeviceSet::setAddress(BTAddress address) {
+    m_address = address;
+}
+
+void BTAdvertisedDeviceSet::setCOD(uint32_t cod) {
+    m_cod     = cod;
+	m_haveCOD = true;
+}
+
+void BTAdvertisedDeviceSet::setName(std::string name) {
+    m_name     = name;
+	m_haveName = true;
+}
+
+void BTAdvertisedDeviceSet::setRSSI(int8_t rssi) {
+    m_rssi     = rssi;
+	m_haveRSSI = true;
+}
+
+#endif /* CONFIG_BT_ENABLED */
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
new file mode 100644
index 000000000..3650d4162
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
@@ -0,0 +1,42 @@
+/*
+ * BTScan.h
+ *
+ *  Created on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+
+#ifndef __BTSCAN_H__
+#define __BTSCAN_H__
+
+#include <map>
+#include <string>
+#include <Print.h>
+#include "BTAddress.h"
+#include "BTAdvertisedDevice.h"
+
+class BTAdvertisedDevice;
+class BTAdvertisedDeviceSet;
+
+
+class BTScanResults {
+public:
+	virtual ~BTScanResults() = default;
+
+    virtual void        		dump(Print *print = nullptr);
+    virtual int         		getCount();
+    virtual BTAdvertisedDevice* getDevice(uint32_t i);
+};
+
+class BTScanResultsSet : public BTScanResults {
+public:
+    void                	dump(Print *print = nullptr);
+    int                 	getCount();
+    BTAdvertisedDevice*		getDevice(uint32_t i);
+
+	bool add(BTAdvertisedDeviceSet advertisedDevice, bool unique = true);
+	void clear();
+
+    std::map<std::string, BTAdvertisedDeviceSet> m_vectorAdvertisedDevices;
+};
+
+#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp
new file mode 100644
index 000000000..c4d00fa42
--- /dev/null
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp
@@ -0,0 +1,95 @@
+/*
+ * BTScanResultsSet.cpp
+ *
+ *  Created on: Feb 5, 2021
+ *      Author: Thomas M. (ArcticSnowSky)
+ */
+
+#include "sdkconfig.h"
+#if defined(CONFIG_BT_ENABLED)
+
+
+#include <esp_err.h>
+
+#include "BTAdvertisedDevice.h"
+#include "BTScan.h"
+//#include "GeneralUtils.h"
+#include "esp32-hal-log.h"
+
+
+class BTAdvertisedDevice;
+
+/**
+ * @brief Dump the scan results to the log.
+ */
+void BTScanResultsSet::dump(Print *print) {
+	int cnt = getCount();
+	if (print == nullptr) {
+		log_v(">> Dump scan results : %d", cnt);
+		for (int i=0; i < cnt; i++) {
+			BTAdvertisedDevice* dev = getDevice(i);
+			if (dev)
+				log_d("- %d: %s\n", i+1, dev->toString().c_str());
+			else
+				log_d("- %d is null\n", i+1);
+		}
+		log_v("-- dump finished --");
+	} else {
+		print->printf(">> Dump scan results: %d\n", cnt);
+		for (int i=0; i < cnt; i++) {
+			BTAdvertisedDevice* dev = getDevice(i);
+			if (dev)
+				print->printf("- %d: %s\n", i+1, dev->toString().c_str());
+			else
+				print->printf("- %d is null\n", i+1);
+		}
+		print->println("-- Dump finished --");
+	}
+} // dump
+
+
+/**
+ * @brief Return the count of devices found in the last scan.
+ * @return The number of devices found in the last scan.
+ */
+int BTScanResultsSet::getCount() {
+	return m_vectorAdvertisedDevices.size();
+} // getCount
+
+
+/**
+ * @brief Return the specified device at the given index.
+ * The index should be between 0 and getCount()-1.
+ * @param [in] i The index of the device.
+ * @return The device at the specified index.
+ */
+BTAdvertisedDevice* BTScanResultsSet::getDevice(uint32_t i) {
+	if (i < 0)
+		return nullptr;
+
+	uint32_t x = 0;
+	BTAdvertisedDeviceSet* pDev = &m_vectorAdvertisedDevices.begin()->second;
+	for (auto it = m_vectorAdvertisedDevices.begin(); it != m_vectorAdvertisedDevices.end(); it++) {
+		pDev = &it->second;
+		if (x==i)	break;
+		x++;
+	}
+	return x==i ? pDev : nullptr;
+}
+
+void BTScanResultsSet::clear() {
+    //for(auto _dev : m_vectorAdvertisedDevices)
+	//	delete _dev.second;
+	m_vectorAdvertisedDevices.clear();
+}
+
+bool BTScanResultsSet::add(BTAdvertisedDeviceSet advertisedDevice, bool unique) {
+	std::string key = advertisedDevice.getAddress().toString();
+	if (!unique || m_vectorAdvertisedDevices.count(key) == 0) {
+		m_vectorAdvertisedDevices.insert(std::pair<std::string, BTAdvertisedDeviceSet>(key, advertisedDevice));
+		return true;
+	} else
+		return false;
+}
+
+#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
index 966487b3c..26f304ef8 100644
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
@@ -42,8 +42,9 @@ const char * _spp_server_name = "ESP32SPP";
 
 //Now passed in during begin()
 //#define RX_QUEUE_SIZE (512 * 4) //Increase to facilitate larger NTRIP transfers
-//#define RX_QUEUE_SIZE 512
 //#define TX_QUEUE_SIZE 512 //Increase to facilitate high transmission rates
+
+//#define RX_QUEUE_SIZE 512 //Original
 //#define TX_QUEUE_SIZE 32
 #define SPP_TX_QUEUE_TIMEOUT 1000
 #define SPP_TX_DONE_TIMEOUT 1000
@@ -55,6 +56,7 @@ static xQueueHandle _spp_tx_queue = NULL;
 static SemaphoreHandle_t _spp_tx_done = NULL;
 static TaskHandle_t _spp_task_handle = NULL;
 static EventGroupHandle_t _spp_event_group = NULL;
+static EventGroupHandle_t _bt_event_group = NULL;
 static boolean secondConnectionAttempt;
 static esp_spp_cb_t * custom_spp_callback = NULL;
 static BluetoothSerialDataCb custom_data_callback = NULL;
@@ -75,11 +77,18 @@ static int _pin_len;
 static bool _isPinSet;
 static bool _enableSSP;
 
+static BTScanResultsSet scanResults;
+static BTAdvertisedDeviceCb advertisedDeviceCb = nullptr;
+
 #define SPP_RUNNING     0x01
 #define SPP_CONNECTED   0x02
 #define SPP_CONGESTED   0x04
 #define SPP_DISCONNECTED 0x08
 
+#define BT_DISCOVERY_RUNNING    0x01
+#define BT_DISCOVERY_COMPLETED  0x02
+
+
 typedef struct {
         size_t len;
         uint8_t data[];
@@ -170,7 +179,7 @@ static esp_err_t _spp_queue_packet(uint8_t *data, size_t len){
 
 //const uint16_t SPP_TX_MAX = 330; //Original 
 const uint16_t SPP_TX_MAX = 1024*2; //Should match the SERIAL_SIZE_RX buffer size in RTK_Surveyor.ino
-static uint8_t _spp_tx_buffer[SPP_TX_MAX]; 
+static uint8_t _spp_tx_buffer[SPP_TX_MAX];
 static uint16_t _spp_tx_buffer_len = 0;
 
 static bool _spp_send_buffer(){
@@ -255,7 +264,11 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
     {
     case ESP_SPP_INIT_EVT:
         log_i("ESP_SPP_INIT_EVT");
+#ifdef ESP_IDF_VERSION_MAJOR
+        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
+#else
         esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
+#endif
         if (!_isMaster) {
             log_i("ESP_SPP_INIT_EVT: slave: start");
             esp_spp_start_srv(ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE, 0, _spp_server_name);
@@ -299,13 +312,10 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
     case ESP_SPP_CONG_EVT://connection congestion status changed
         if(param->cong.cong){
             xEventGroupClearBits(_spp_event_group, SPP_CONGESTED);
-            log_d("ESP_SPP_CONG_EVT: CONGESTED");
-        }
-        else
-        {
+        } else {
             xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
         }
-        //log_v("ESP_SPP_CONG_EVT: %s", param->cong.cong ? "CONGESTED" : "FREE");
+        log_v("ESP_SPP_CONG_EVT: %s", param->cong.cong?"CONGESTED":"FREE");
         break;
 
     case ESP_SPP_WRITE_EVT://write operation completed
@@ -381,15 +391,16 @@ void BluetoothSerial::onData(BluetoothSerialDataCb cb){
 static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
 {
     switch(event){
-        case ESP_BT_GAP_DISC_RES_EVT:
+        case ESP_BT_GAP_DISC_RES_EVT: {
             log_i("ESP_BT_GAP_DISC_RES_EVT");
 #if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
             char bda_str[18];
             log_i("Scanned device: %s", bda2str(param->disc_res.bda, bda_str, 18));
 #endif
+            BTAdvertisedDeviceSet advertisedDevice;
+            uint8_t peer_bdname_len = 0;
+            char peer_bdname[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
             for (int i = 0; i < param->disc_res.num_prop; i++) {
-                uint8_t peer_bdname_len;
-                char peer_bdname[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
                 switch(param->disc_res.prop[i].type) {
                     case ESP_BT_GAP_DEV_PROP_EIR:  
                         if (get_name_from_eir((uint8_t*)param->disc_res.prop[i].val, peer_bdname, &peer_bdname_len)) {
@@ -422,10 +433,24 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
 
                     case ESP_BT_GAP_DEV_PROP_COD:
                         log_d("ESP_BT_GAP_DEV_PROP_COD");
+                        if (param->disc_res.prop[i].len <= sizeof(int)) {
+                            uint32_t cod = 0;
+                            memcpy(&cod, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
+                            advertisedDevice.setCOD(cod);
+                        } else {
+                            log_d("Value size larger than integer");
+                        }
                         break;
 
                     case ESP_BT_GAP_DEV_PROP_RSSI:
                         log_d("ESP_BT_GAP_DEV_PROP_RSSI");
+                        if (param->disc_res.prop[i].len <= sizeof(int)) {
+                            uint8_t rssi = 0;
+                            memcpy(&rssi, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
+                            advertisedDevice.setRSSI(rssi);
+                        } else {
+                            log_d("Value size larger than integer");
+                        }
                         break;
                         
                     default:
@@ -434,17 +459,33 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
                 if (_isRemoteAddressSet)
                     break;
             }
-            break;
+            if (peer_bdname_len)
+                advertisedDevice.setName(peer_bdname);
+            esp_bd_addr_t addr;
+            memcpy(addr, param->disc_res.bda, ESP_BD_ADDR_LEN);
+            advertisedDevice.setAddress(BTAddress(addr));
+            if (scanResults.add(advertisedDevice) && advertisedDeviceCb)
+                advertisedDeviceCb(&advertisedDevice);
+        }
+        break;
+
         case ESP_BT_GAP_DISC_STATE_CHANGED_EVT:
             log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT");
+            if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STOPPED) {
+                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_RUNNING);
+                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
+            } else { // ESP_BT_GAP_DISCOVERY_STARTED
+                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
+                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_RUNNING);
+            }
             break;
 
         case ESP_BT_GAP_RMT_SRVCS_EVT:
-            log_i( "ESP_BT_GAP_RMT_SRVCS_EVT");
+            log_i( "ESP_BT_GAP_RMT_SRVCS_EVT: status = %d, num_uuids = %d", param->rmt_srvcs.stat, param->rmt_srvcs.num_uuids);
             break;
 
         case ESP_BT_GAP_RMT_SRVC_REC_EVT:
-            log_i("ESP_BT_GAP_RMT_SRVC_REC_EVT");
+            log_i("ESP_BT_GAP_RMT_SRVC_REC_EVT: status = %d", param->rmt_srvc_rec.stat);
             break;
 
         case ESP_BT_GAP_AUTH_CMPL_EVT:
@@ -503,6 +544,14 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
 
 static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQueueSize)
 {
+    if(!_bt_event_group){
+        _bt_event_group = xEventGroupCreate();
+        if(!_bt_event_group){
+            log_e("BT Event Group Create Failed!");
+            return false;
+        }
+        xEventGroupClearBits(_bt_event_group, 0xFFFFFF);
+    }
     if(!_spp_event_group){
         _spp_event_group = xEventGroupCreate();
         if(!_spp_event_group){
@@ -521,7 +570,7 @@ static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQu
         }
     }
     if (_spp_tx_queue == NULL){
-        _spp_tx_queue = xQueueCreate(txQueueSize, sizeof(spp_packet_t *)); //initialize the queue
+        _spp_tx_queue = xQueueCreate(txQueueSize, sizeof(spp_packet_t*)); //initialize the queue
         if (_spp_tx_queue == NULL){
             log_e("TX Queue Create Failed");
             return false;
@@ -537,7 +586,7 @@ static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQu
     }
 
     if(!_spp_task_handle){
-        xTaskCreatePinnedToCore(_spp_tx_task, "spp_tx", 4096, NULL, 2, &_spp_task_handle, 0);
+        xTaskCreatePinnedToCore(_spp_tx_task, "spp_tx", 4096, NULL, 10, &_spp_task_handle, 0);
         if(!_spp_task_handle){
             log_e("Network Event Task Start Failed!");
             return false;
@@ -647,6 +696,10 @@ static bool _stop_bt()
         vSemaphoreDelete(_spp_tx_done);
         _spp_tx_done = NULL;
     }
+    if (_bt_event_group) {
+        vEventGroupDelete(_bt_event_group);
+        _bt_event_group = NULL;
+    }
     return true;
 }
 
@@ -655,6 +708,11 @@ static bool waitForConnect(int timeout) {
     return (xEventGroupWaitBits(_spp_event_group, SPP_CONNECTED, pdFALSE, pdTRUE, xTicksToWait) & SPP_CONNECTED) != 0;
 }
 
+static bool waitForDiscovered(int timeout) {
+    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
+    return (xEventGroupWaitBits(_spp_event_group, BT_DISCOVERY_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_DISCOVERY_COMPLETED) != 0;
+}
+
 /*
  * Serial Bluetooth Arduino
  *
@@ -799,7 +857,11 @@ bool BluetoothSerial::connect(String remoteName)
     _remote_name[ESP_BT_GAP_MAX_BDNAME_LEN] = 0;
     log_i("master : remoteName");
     // will first resolve name to address
-    esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
+#ifdef ESP_IDF_VERSION_MAJOR
+        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
+#else
+        esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
+#endif
     if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
         return waitForConnect(SCAN_TIMEOUT);
     }
@@ -839,7 +901,11 @@ bool BluetoothSerial::connect()
         disconnect();
         log_i("master : remoteName");
         // will resolve name to address first - it may take a while
+#ifdef ESP_IDF_VERSION_MAJOR
+        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
+#else
         esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
+#endif
         if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
             return waitForConnect(SCAN_TIMEOUT);
         }
@@ -886,8 +952,75 @@ bool BluetoothSerial::isReady(bool checkMaster, int timeout) {
     return (xEventGroupWaitBits(_spp_event_group, SPP_RUNNING, pdFALSE, pdTRUE, xTicksToWait) & SPP_RUNNING) != 0;
 }
 
-bool BluetoothSerial::isCongested(){
-    return(!(xEventGroupGetBits(_spp_event_group) & SPP_CONGESTED));
+
+/**
+ * @brief           RemoteName or address are not allowed to be set during discovery
+ *                  (otherwhise it might connect automatically and stop discovery)
+ * @param[in]       timeoutMs can range from MIN_INQ_TIME to MAX_INQ_TIME
+ * @return          in case of Error immediately Empty ScanResults.
+ */
+BTScanResults* BluetoothSerial::discover(int timeoutMs) {
+    scanResults.clear();
+    if (timeoutMs < MIN_INQ_TIME || timeoutMs > MAX_INQ_TIME || strlen(_remote_name) || _isRemoteAddressSet)
+        return nullptr;
+    int timeout = timeoutMs / INQ_TIME;
+    log_i("discover::disconnect");
+    disconnect();
+    log_i("discovering");
+    // will resolve name to address first - it may take a while
+    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
+    if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK) {
+        waitForDiscovered(timeoutMs);
+        esp_bt_gap_cancel_discovery();
+    }
+    return &scanResults;
+}
+
+/**
+ * @brief           RemoteName or address are not allowed to be set during discovery
+ *                  (otherwhise it might connect automatically and stop discovery)
+ * @param[in]       cb called when a [b]new[/b] device has been discovered
+ * @param[in]       timeoutMs can be 0 or range from MIN_INQ_TIME to MAX_INQ_TIME
+ *
+ * @return          Wheter start was successfull or problems with params
+ */
+bool BluetoothSerial::discoverAsync(BTAdvertisedDeviceCb cb, int timeoutMs) {
+    scanResults.clear();
+    if (strlen(_remote_name) || _isRemoteAddressSet)
+        return false;
+    int timeout = timeoutMs / INQ_TIME;
+    disconnect();
+    advertisedDeviceCb = cb;
+    log_i("discovering");
+    // will resolve name to address first - it may take a while
+    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
+    if (timeout > 0)
+        return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK;
+    else return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, ESP_BT_GAP_MAX_INQ_LEN, 0) == ESP_OK;
 }
 
+/** @brief      Stops the asynchronous discovery and clears the callback */
+void BluetoothSerial::discoverAsyncStop() {
+    esp_bt_gap_cancel_discovery();
+    advertisedDeviceCb = nullptr;
+}
+
+/** @brief      Clears scanresult entries */
+void BluetoothSerial::discoverClear() {
+    scanResults.clear();
+}
+
+/** @brief      Can be used while discovering asynchronously
+ *              Will be returned also on synchronous discovery.
+ *
+ * @return      BTScanResults contains several information of found devices
+ */
+BTScanResults* BluetoothSerial::getScanResults() {
+    return &scanResults;
+}
+
+BluetoothSerial::operator bool() const
+{
+    return true;
+}
 #endif
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
index 6c4d2d3e3..d7d52bd21 100644
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
@@ -21,12 +21,15 @@
 
 #include "Arduino.h"
 #include "Stream.h"
+#include <esp_gap_bt_api.h>
 #include <esp_spp_api.h>
 #include <functional>
+#include "BTScan.h"
 
 typedef std::function<void(const uint8_t *buffer, size_t size)> BluetoothSerialDataCb;
 typedef std::function<void(uint32_t num_val)> ConfirmRequestCb;
 typedef std::function<void(boolean success)> AuthCompleteCb;
+typedef std::function<void(BTAdvertisedDevice* pAdvertisedDevice)> BTAdvertisedDeviceCb;
 
 class BluetoothSerial: public Stream
 {
@@ -36,6 +39,9 @@ class BluetoothSerial: public Stream
         ~BluetoothSerial(void);
 
         bool begin(String localName=String(), bool isMaster=false, uint16_t rxQueueSize = 512 * 4, uint16_t txQueueSize = 512);
+        bool begin(unsigned long baud){//compatibility
+            return begin();
+        }
         int available(void);
         int peek(void);
         bool hasClient(void);
@@ -61,8 +67,17 @@ class BluetoothSerial: public Stream
         bool disconnect();
         bool unpairDevice(uint8_t remoteAddress[]);
 
-        bool isCongested();
-
+        BTScanResults* discover(int timeout=0x30*1280);
+        bool discoverAsync(BTAdvertisedDeviceCb cb, int timeout=0x30*1280);
+        void discoverAsyncStop();
+        void discoverClear();
+        BTScanResults* getScanResults();
+        
+        const int INQ_TIME = 1280;   // Inquire Time unit 1280 ms
+        const int MIN_INQ_TIME = (ESP_BT_GAP_MIN_INQ_LEN * INQ_TIME);
+        const int MAX_INQ_TIME = (ESP_BT_GAP_MAX_INQ_LEN * INQ_TIME);
+        
+        operator bool() const;
     private:
         String local_name;
 

From c4165cb756a6fe189a55783f6f03d554da42673d Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 10:52:16 -0600
Subject: [PATCH 11/17] Set rxBufferSize before starting UART.

---
 Firmware/RTK_Surveyor/Begin.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/RTK_Surveyor/Begin.ino b/Firmware/RTK_Surveyor/Begin.ino
index c763f2eec..4b19d57eb 100644
--- a/Firmware/RTK_Surveyor/Begin.ino
+++ b/Firmware/RTK_Surveyor/Begin.ino
@@ -239,9 +239,9 @@ void beginUART2()
 //Assign UART2 interrupts to the core 0. See: https://github.com/espressif/arduino-esp32/issues/3386
 void pinUART2Task( void *pvParameters )
 {
-  serialGNSS.begin(settings.dataPortBaud); //UART2 on pins 16/17 for SPP. The ZED-F9P will be configured to output NMEA over its UART1 at the same rate.
   serialGNSS.setRxBufferSize(SERIAL_SIZE_RX);
   serialGNSS.setTimeout(50);
+  serialGNSS.begin(settings.dataPortBaud); //UART2 on pins 16/17 for SPP. The ZED-F9P will be configured to output NMEA over its UART1 at the same rate.
 
   uart2pinned = true;
 

From 04b80e71943b589c847c8877b08f4d4255440dc0 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 12:03:31 -0600
Subject: [PATCH 12/17] Fix factory reset from rolling reset.

Increase readTaskStackSize 2000 to 2500 to prevent factoryReset from failing. Change uart2Pinned var to volatile as it is accessed by both cores.
---
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 7 ++++---
 Firmware/RTK_Surveyor/System.ino       | 4 ++--
 Firmware/RTK_Surveyor/menuMain.ino     | 4 ++--
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index 3249b8f02..61eeeec1d 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -224,7 +224,7 @@ TaskHandle_t pinUART2TaskHandle = NULL; //Dummy task to start UART2 on core 0.
 bool uart2pinned = false;
 
 //Reduced stack size from 10,000 to 2,000 to make room for WiFi/NTRIP server capabilities
-const int readTaskStackSize = 2000;
+const int readTaskStackSize = 2500;
 const int writeTaskStackSize = 2000;
 
 char incomingBTTest = 0; //Stores incoming text over BT when in test mode
@@ -282,7 +282,7 @@ unsigned long lastRockerSwitchChange = 0; //If quick toggle is detected (less th
 //=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=
 #ifdef COMPILE_WIFI
 
-#include "ESPAsyncWebServer.h"
+#include "ESPAsyncWebServer.h" //Get from: https://github.com/me-no-dev/ESPAsyncWebServer 
 #include "form.h"
 
 AsyncWebServer server(80);
@@ -356,7 +356,7 @@ void setup()
   Serial.begin(115200); //UART0 for programming and debugging
 
   Wire.begin(); //Start I2C on core 1
-  Wire.setClock(100000); //Confirm we are at 100kHz
+  Wire.setClock(400000); //Increase bus rate to 400kHz
 
   beginGNSS(); //Connect to GNSS
 
@@ -483,6 +483,7 @@ void updateLogs()
       }
     }
   }
+  //  }
 }
 
 //Once we have a fix, sync system clock to GNSS
diff --git a/Firmware/RTK_Surveyor/System.ino b/Firmware/RTK_Surveyor/System.ino
index f3330f474..56d44ed83 100644
--- a/Firmware/RTK_Surveyor/System.ino
+++ b/Firmware/RTK_Surveyor/System.ino
@@ -597,9 +597,9 @@ bool createTestFile()
   SdFile testFile;
   char testFileName[40] = "testfile.txt";
 
-  if(online.microSD == false)
+  if(xFATSemaphore == NULL)
   {
-    Serial.println(F("SD card offline"));
+    ESP_LOGD(TAG, "xFATSemaphote is Null");
     return(false);
   }
 
diff --git a/Firmware/RTK_Surveyor/menuMain.ino b/Firmware/RTK_Surveyor/menuMain.ino
index 9ce21bc4b..5d0103931 100644
--- a/Firmware/RTK_Surveyor/menuMain.ino
+++ b/Firmware/RTK_Surveyor/menuMain.ino
@@ -122,10 +122,10 @@ void factoryReset()
     } //End xFATSemaphore
   }
 
-  i2cGNSS.factoryReset(); //Reset everything: baud rate, I2C address, update rate, everything.
-
   displaySytemReset(); //Display friendly message on OLED
 
+  i2cGNSS.factoryReset(); //Reset everything: baud rate, I2C address, update rate, everything.
+
   Serial.println(F("Settings erased successfully. Rebooting. Good bye!"));
   delay(2000);
   ESP.restart();

From feb32147e24c0b2895d1215542b5e40dc9b8e222 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 12:07:29 -0600
Subject: [PATCH 13/17] Revert back to v1.0.6 of the core.

Writing to files in the F9Read task is causing core panic. Not sure how v2 of core changed how tasks interact with SPI transactions of sdfat.
---
 .../src/BluetoothSerial/BTAddress.cpp         |  96 ----------
 .../src/BluetoothSerial/BTAddress.h           |  36 ----
 .../src/BluetoothSerial/BTAdvertisedDevice.h  |  65 -------
 .../BluetoothSerial/BTAdvertisedDeviceSet.cpp |  78 --------
 .../RTK_Surveyor/src/BluetoothSerial/BTScan.h |  42 -----
 .../src/BluetoothSerial/BTScanResultsSet.cpp  |  95 ----------
 .../src/BluetoothSerial/BluetoothSerial.cpp   | 169 ++----------------
 .../src/BluetoothSerial/BluetoothSerial.h     |  19 +-
 8 files changed, 20 insertions(+), 580 deletions(-)
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
 delete mode 100644 Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp

diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
deleted file mode 100644
index 1ed416f2b..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.cpp
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
- * BTAddress.cpp
- *
- *  Created on: Jul 2, 2017
- *      Author: kolban
- *  Ported  on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-#include "BTAddress.h"
-#include <string>
-#include <sstream>
-#include <iomanip>
-#include <string.h>
-#include <stdio.h>
-#include <malloc.h>
-#ifdef ARDUINO_ARCH_ESP32
-#include "esp32-hal-log.h"
-#endif
-
-
-/**
- * @brief Create an address from the native ESP32 representation.
- * @param [in] address The native representation.
- */
-BTAddress::BTAddress(esp_bd_addr_t address) {
-	memcpy(m_address, address, ESP_BD_ADDR_LEN);
-} // BTAddress
-
-
-/**
- * @brief Create an address from a hex string
- *
- * A hex string is of the format:
- * ```
- * 00:00:00:00:00:00
- * ```
- * which is 17 characters in length.
- *
- * @param [in] stringAddress The hex representation of the address.
- */
-BTAddress::BTAddress(std::string stringAddress) {
-	if (stringAddress.length() != 17) return;
-
-	int data[6];
-	sscanf(stringAddress.c_str(), "%x:%x:%x:%x:%x:%x", &data[0], &data[1], &data[2], &data[3], &data[4], &data[5]);
-	m_address[0] = (uint8_t) data[0];
-	m_address[1] = (uint8_t) data[1];
-	m_address[2] = (uint8_t) data[2];
-	m_address[3] = (uint8_t) data[3];
-	m_address[4] = (uint8_t) data[4];
-	m_address[5] = (uint8_t) data[5];
-} // BTAddress
-
-
-/**
- * @brief Determine if this address equals another.
- * @param [in] otherAddress The other address to compare against.
- * @return True if the addresses are equal.
- */
-bool BTAddress::equals(BTAddress otherAddress) {
-	return memcmp(otherAddress.getNative(), m_address, 6) == 0;
-} // equals
-
-
-/**
- * @brief Return the native representation of the address.
- * @return The native representation of the address.
- */
-esp_bd_addr_t *BTAddress::getNative() {
-	return &m_address;
-} // getNative
-
-
-/**
- * @brief Convert a BT address to a string.
- *
- * A string representation of an address is in the format:
- *
- * ```
- * xx:xx:xx:xx:xx:xx
- * ```
- *
- * @return The string representation of the address.
- */
-std::string BTAddress::toString() {
-	auto size = 18;
-	char *res = (char*)malloc(size);
-	snprintf(res, size, "%02x:%02x:%02x:%02x:%02x:%02x", m_address[0], m_address[1], m_address[2], m_address[3], m_address[4], m_address[5]);
-	std::string ret(res);
-	free(res);
-	return ret;
-} // toString
-#endif
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
deleted file mode 100644
index e23d34593..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAddress.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*
- * BTAddress.h
- *
- *  Created on: Jul 2, 2017
- *      Author: kolban
- *  Ported  on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-
-#ifndef COMPONENTS_CPP_UTILS_BTADDRESS_H_
-#define COMPONENTS_CPP_UTILS_BTADDRESS_H_
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-#include <esp_gap_bt_api.h> // ESP32 BT
-#include <string>
-
-
-/**
- * @brief A %BT device address.
- *
- * Every %BT device has a unique address which can be used to identify it and form connections.
- */
-class BTAddress {
-public:
-	BTAddress(esp_bd_addr_t address);
-	BTAddress(std::string stringAddress);
-	bool           equals(BTAddress otherAddress);
-	esp_bd_addr_t* getNative();
-	std::string    toString();
-
-private:
-	esp_bd_addr_t m_address;
-};
-
-#endif /* CONFIG_BT_ENABLED */
-#endif /* COMPONENTS_CPP_UTILS_BTADDRESS_H_ */
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
deleted file mode 100644
index 07e93622e..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDevice.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- * BTAdvertisedDevice.h
- *
- *  Created on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-
-#ifndef __BTADVERTISEDDEVICE_H__
-#define __BTADVERTISEDDEVICE_H__
-
-#include "BTAddress.h"
-
-
-class BTAdvertisedDevice {
-public:
-    virtual ~BTAdvertisedDevice() = default;
-
-    virtual BTAddress   getAddress();
-    virtual uint32_t    getCOD();
-    virtual std::string getName();
-    virtual int8_t      getRSSI();
-
-
-    virtual bool        haveCOD();
-    virtual bool        haveName();
-    virtual bool        haveRSSI();
-
-    virtual std::string toString();
-};
-
-class BTAdvertisedDeviceSet : public virtual BTAdvertisedDevice {
-public:
-    BTAdvertisedDeviceSet();
-    //~BTAdvertisedDeviceSet() = default;
-    
-
-    BTAddress   getAddress();
-    uint32_t    getCOD();
-    std::string getName();
-    int8_t      getRSSI();
-
-
-    bool        haveCOD();
-    bool        haveName();
-    bool        haveRSSI();
-
-    std::string toString();
-
-    void setAddress(BTAddress address);
-    void setCOD(uint32_t cod);
-    void setName(std::string name);
-    void setRSSI(int8_t rssi);
-
-    bool m_haveCOD;
-    bool m_haveName;
-    bool m_haveRSSI;
-
-
-    BTAddress   m_address = BTAddress((uint8_t*)"\0\0\0\0\0\0");
-    uint32_t    m_cod;
-    std::string m_name;
-    int8_t      m_rssi;
-};
-
-#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
deleted file mode 100644
index 78df93eab..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTAdvertisedDeviceSet.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/*
- * BTAdvertisedDeviceSet.cpp
- *
- *  Created on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-//#include <map>
-
-#include "BTAdvertisedDevice.h"
-//#include "BTScan.h"
-
-
-BTAdvertisedDeviceSet::BTAdvertisedDeviceSet() {
-	m_cod		       = 0;
-	m_name             = "";
-	m_rssi             = 0;
-
-	m_haveCOD	       = false;
-	m_haveName         = false;
-	m_haveRSSI         = false;
-} // BTAdvertisedDeviceSet
-
-BTAddress   BTAdvertisedDeviceSet::getAddress()    { return m_address; }
-uint32_t    BTAdvertisedDeviceSet::getCOD()        { return m_cod; }
-std::string BTAdvertisedDeviceSet::getName()       { return m_name; }
-int8_t      BTAdvertisedDeviceSet::getRSSI()	    { return m_rssi; }
-
-
-bool        BTAdvertisedDeviceSet::haveCOD()   { return m_haveCOD; }
-bool        BTAdvertisedDeviceSet::haveName()  { return m_haveName; }
-bool        BTAdvertisedDeviceSet::haveRSSI()  { return m_haveRSSI; }
-
-/**
- * @brief Create a string representation of this device.
- * @return A string representation of this device.
- */
-std::string BTAdvertisedDeviceSet::toString() {
-	std::string res = "Name: " + getName() + ", Address: " + getAddress().toString();
-	if (haveCOD()) {
-		char val[6];
-		snprintf(val, sizeof(val), "%d", getCOD());
-		res += ", cod: ";
-		res += val;
-	}
-	if (haveRSSI()) {
-		char val[6];
-		snprintf(val, sizeof(val), "%d", (int8_t)getRSSI());
-		res += ", rssi: ";
-		res += val;
-	}
-	return res;
-} // toString
-
-
-void BTAdvertisedDeviceSet::setAddress(BTAddress address) {
-    m_address = address;
-}
-
-void BTAdvertisedDeviceSet::setCOD(uint32_t cod) {
-    m_cod     = cod;
-	m_haveCOD = true;
-}
-
-void BTAdvertisedDeviceSet::setName(std::string name) {
-    m_name     = name;
-	m_haveName = true;
-}
-
-void BTAdvertisedDeviceSet::setRSSI(int8_t rssi) {
-    m_rssi     = rssi;
-	m_haveRSSI = true;
-}
-
-#endif /* CONFIG_BT_ENABLED */
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
deleted file mode 100644
index 3650d4162..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScan.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*
- * BTScan.h
- *
- *  Created on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-
-#ifndef __BTSCAN_H__
-#define __BTSCAN_H__
-
-#include <map>
-#include <string>
-#include <Print.h>
-#include "BTAddress.h"
-#include "BTAdvertisedDevice.h"
-
-class BTAdvertisedDevice;
-class BTAdvertisedDeviceSet;
-
-
-class BTScanResults {
-public:
-	virtual ~BTScanResults() = default;
-
-    virtual void        		dump(Print *print = nullptr);
-    virtual int         		getCount();
-    virtual BTAdvertisedDevice* getDevice(uint32_t i);
-};
-
-class BTScanResultsSet : public BTScanResults {
-public:
-    void                	dump(Print *print = nullptr);
-    int                 	getCount();
-    BTAdvertisedDevice*		getDevice(uint32_t i);
-
-	bool add(BTAdvertisedDeviceSet advertisedDevice, bool unique = true);
-	void clear();
-
-    std::map<std::string, BTAdvertisedDeviceSet> m_vectorAdvertisedDevices;
-};
-
-#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp
deleted file mode 100644
index c4d00fa42..000000000
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BTScanResultsSet.cpp
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * BTScanResultsSet.cpp
- *
- *  Created on: Feb 5, 2021
- *      Author: Thomas M. (ArcticSnowSky)
- */
-
-#include "sdkconfig.h"
-#if defined(CONFIG_BT_ENABLED)
-
-
-#include <esp_err.h>
-
-#include "BTAdvertisedDevice.h"
-#include "BTScan.h"
-//#include "GeneralUtils.h"
-#include "esp32-hal-log.h"
-
-
-class BTAdvertisedDevice;
-
-/**
- * @brief Dump the scan results to the log.
- */
-void BTScanResultsSet::dump(Print *print) {
-	int cnt = getCount();
-	if (print == nullptr) {
-		log_v(">> Dump scan results : %d", cnt);
-		for (int i=0; i < cnt; i++) {
-			BTAdvertisedDevice* dev = getDevice(i);
-			if (dev)
-				log_d("- %d: %s\n", i+1, dev->toString().c_str());
-			else
-				log_d("- %d is null\n", i+1);
-		}
-		log_v("-- dump finished --");
-	} else {
-		print->printf(">> Dump scan results: %d\n", cnt);
-		for (int i=0; i < cnt; i++) {
-			BTAdvertisedDevice* dev = getDevice(i);
-			if (dev)
-				print->printf("- %d: %s\n", i+1, dev->toString().c_str());
-			else
-				print->printf("- %d is null\n", i+1);
-		}
-		print->println("-- Dump finished --");
-	}
-} // dump
-
-
-/**
- * @brief Return the count of devices found in the last scan.
- * @return The number of devices found in the last scan.
- */
-int BTScanResultsSet::getCount() {
-	return m_vectorAdvertisedDevices.size();
-} // getCount
-
-
-/**
- * @brief Return the specified device at the given index.
- * The index should be between 0 and getCount()-1.
- * @param [in] i The index of the device.
- * @return The device at the specified index.
- */
-BTAdvertisedDevice* BTScanResultsSet::getDevice(uint32_t i) {
-	if (i < 0)
-		return nullptr;
-
-	uint32_t x = 0;
-	BTAdvertisedDeviceSet* pDev = &m_vectorAdvertisedDevices.begin()->second;
-	for (auto it = m_vectorAdvertisedDevices.begin(); it != m_vectorAdvertisedDevices.end(); it++) {
-		pDev = &it->second;
-		if (x==i)	break;
-		x++;
-	}
-	return x==i ? pDev : nullptr;
-}
-
-void BTScanResultsSet::clear() {
-    //for(auto _dev : m_vectorAdvertisedDevices)
-	//	delete _dev.second;
-	m_vectorAdvertisedDevices.clear();
-}
-
-bool BTScanResultsSet::add(BTAdvertisedDeviceSet advertisedDevice, bool unique) {
-	std::string key = advertisedDevice.getAddress().toString();
-	if (!unique || m_vectorAdvertisedDevices.count(key) == 0) {
-		m_vectorAdvertisedDevices.insert(std::pair<std::string, BTAdvertisedDeviceSet>(key, advertisedDevice));
-		return true;
-	} else
-		return false;
-}
-
-#endif
\ No newline at end of file
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
index 26f304ef8..966487b3c 100644
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.cpp
@@ -42,9 +42,8 @@ const char * _spp_server_name = "ESP32SPP";
 
 //Now passed in during begin()
 //#define RX_QUEUE_SIZE (512 * 4) //Increase to facilitate larger NTRIP transfers
+//#define RX_QUEUE_SIZE 512
 //#define TX_QUEUE_SIZE 512 //Increase to facilitate high transmission rates
-
-//#define RX_QUEUE_SIZE 512 //Original
 //#define TX_QUEUE_SIZE 32
 #define SPP_TX_QUEUE_TIMEOUT 1000
 #define SPP_TX_DONE_TIMEOUT 1000
@@ -56,7 +55,6 @@ static xQueueHandle _spp_tx_queue = NULL;
 static SemaphoreHandle_t _spp_tx_done = NULL;
 static TaskHandle_t _spp_task_handle = NULL;
 static EventGroupHandle_t _spp_event_group = NULL;
-static EventGroupHandle_t _bt_event_group = NULL;
 static boolean secondConnectionAttempt;
 static esp_spp_cb_t * custom_spp_callback = NULL;
 static BluetoothSerialDataCb custom_data_callback = NULL;
@@ -77,18 +75,11 @@ static int _pin_len;
 static bool _isPinSet;
 static bool _enableSSP;
 
-static BTScanResultsSet scanResults;
-static BTAdvertisedDeviceCb advertisedDeviceCb = nullptr;
-
 #define SPP_RUNNING     0x01
 #define SPP_CONNECTED   0x02
 #define SPP_CONGESTED   0x04
 #define SPP_DISCONNECTED 0x08
 
-#define BT_DISCOVERY_RUNNING    0x01
-#define BT_DISCOVERY_COMPLETED  0x02
-
-
 typedef struct {
         size_t len;
         uint8_t data[];
@@ -179,7 +170,7 @@ static esp_err_t _spp_queue_packet(uint8_t *data, size_t len){
 
 //const uint16_t SPP_TX_MAX = 330; //Original 
 const uint16_t SPP_TX_MAX = 1024*2; //Should match the SERIAL_SIZE_RX buffer size in RTK_Surveyor.ino
-static uint8_t _spp_tx_buffer[SPP_TX_MAX];
+static uint8_t _spp_tx_buffer[SPP_TX_MAX]; 
 static uint16_t _spp_tx_buffer_len = 0;
 
 static bool _spp_send_buffer(){
@@ -264,11 +255,7 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
     {
     case ESP_SPP_INIT_EVT:
         log_i("ESP_SPP_INIT_EVT");
-#ifdef ESP_IDF_VERSION_MAJOR
-        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
-#else
         esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
-#endif
         if (!_isMaster) {
             log_i("ESP_SPP_INIT_EVT: slave: start");
             esp_spp_start_srv(ESP_SPP_SEC_NONE, ESP_SPP_ROLE_SLAVE, 0, _spp_server_name);
@@ -312,10 +299,13 @@ static void esp_spp_cb(esp_spp_cb_event_t event, esp_spp_cb_param_t *param)
     case ESP_SPP_CONG_EVT://connection congestion status changed
         if(param->cong.cong){
             xEventGroupClearBits(_spp_event_group, SPP_CONGESTED);
-        } else {
+            log_d("ESP_SPP_CONG_EVT: CONGESTED");
+        }
+        else
+        {
             xEventGroupSetBits(_spp_event_group, SPP_CONGESTED);
         }
-        log_v("ESP_SPP_CONG_EVT: %s", param->cong.cong?"CONGESTED":"FREE");
+        //log_v("ESP_SPP_CONG_EVT: %s", param->cong.cong ? "CONGESTED" : "FREE");
         break;
 
     case ESP_SPP_WRITE_EVT://write operation completed
@@ -391,16 +381,15 @@ void BluetoothSerial::onData(BluetoothSerialDataCb cb){
 static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *param)
 {
     switch(event){
-        case ESP_BT_GAP_DISC_RES_EVT: {
+        case ESP_BT_GAP_DISC_RES_EVT:
             log_i("ESP_BT_GAP_DISC_RES_EVT");
 #if (ARDUHAL_LOG_LEVEL >= ARDUHAL_LOG_LEVEL_INFO)
             char bda_str[18];
             log_i("Scanned device: %s", bda2str(param->disc_res.bda, bda_str, 18));
 #endif
-            BTAdvertisedDeviceSet advertisedDevice;
-            uint8_t peer_bdname_len = 0;
-            char peer_bdname[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
             for (int i = 0; i < param->disc_res.num_prop; i++) {
+                uint8_t peer_bdname_len;
+                char peer_bdname[ESP_BT_GAP_MAX_BDNAME_LEN + 1];
                 switch(param->disc_res.prop[i].type) {
                     case ESP_BT_GAP_DEV_PROP_EIR:  
                         if (get_name_from_eir((uint8_t*)param->disc_res.prop[i].val, peer_bdname, &peer_bdname_len)) {
@@ -433,24 +422,10 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
 
                     case ESP_BT_GAP_DEV_PROP_COD:
                         log_d("ESP_BT_GAP_DEV_PROP_COD");
-                        if (param->disc_res.prop[i].len <= sizeof(int)) {
-                            uint32_t cod = 0;
-                            memcpy(&cod, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
-                            advertisedDevice.setCOD(cod);
-                        } else {
-                            log_d("Value size larger than integer");
-                        }
                         break;
 
                     case ESP_BT_GAP_DEV_PROP_RSSI:
                         log_d("ESP_BT_GAP_DEV_PROP_RSSI");
-                        if (param->disc_res.prop[i].len <= sizeof(int)) {
-                            uint8_t rssi = 0;
-                            memcpy(&rssi, param->disc_res.prop[i].val, param->disc_res.prop[i].len);
-                            advertisedDevice.setRSSI(rssi);
-                        } else {
-                            log_d("Value size larger than integer");
-                        }
                         break;
                         
                     default:
@@ -459,33 +434,17 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
                 if (_isRemoteAddressSet)
                     break;
             }
-            if (peer_bdname_len)
-                advertisedDevice.setName(peer_bdname);
-            esp_bd_addr_t addr;
-            memcpy(addr, param->disc_res.bda, ESP_BD_ADDR_LEN);
-            advertisedDevice.setAddress(BTAddress(addr));
-            if (scanResults.add(advertisedDevice) && advertisedDeviceCb)
-                advertisedDeviceCb(&advertisedDevice);
-        }
-        break;
-
+            break;
         case ESP_BT_GAP_DISC_STATE_CHANGED_EVT:
             log_i("ESP_BT_GAP_DISC_STATE_CHANGED_EVT");
-            if (param->disc_st_chg.state == ESP_BT_GAP_DISCOVERY_STOPPED) {
-                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_RUNNING);
-                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
-            } else { // ESP_BT_GAP_DISCOVERY_STARTED
-                xEventGroupClearBits(_bt_event_group, BT_DISCOVERY_COMPLETED);
-                xEventGroupSetBits(_bt_event_group, BT_DISCOVERY_RUNNING);
-            }
             break;
 
         case ESP_BT_GAP_RMT_SRVCS_EVT:
-            log_i( "ESP_BT_GAP_RMT_SRVCS_EVT: status = %d, num_uuids = %d", param->rmt_srvcs.stat, param->rmt_srvcs.num_uuids);
+            log_i( "ESP_BT_GAP_RMT_SRVCS_EVT");
             break;
 
         case ESP_BT_GAP_RMT_SRVC_REC_EVT:
-            log_i("ESP_BT_GAP_RMT_SRVC_REC_EVT: status = %d", param->rmt_srvc_rec.stat);
+            log_i("ESP_BT_GAP_RMT_SRVC_REC_EVT");
             break;
 
         case ESP_BT_GAP_AUTH_CMPL_EVT:
@@ -544,14 +503,6 @@ static void esp_bt_gap_cb(esp_bt_gap_cb_event_t event, esp_bt_gap_cb_param_t *pa
 
 static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQueueSize)
 {
-    if(!_bt_event_group){
-        _bt_event_group = xEventGroupCreate();
-        if(!_bt_event_group){
-            log_e("BT Event Group Create Failed!");
-            return false;
-        }
-        xEventGroupClearBits(_bt_event_group, 0xFFFFFF);
-    }
     if(!_spp_event_group){
         _spp_event_group = xEventGroupCreate();
         if(!_spp_event_group){
@@ -570,7 +521,7 @@ static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQu
         }
     }
     if (_spp_tx_queue == NULL){
-        _spp_tx_queue = xQueueCreate(txQueueSize, sizeof(spp_packet_t*)); //initialize the queue
+        _spp_tx_queue = xQueueCreate(txQueueSize, sizeof(spp_packet_t *)); //initialize the queue
         if (_spp_tx_queue == NULL){
             log_e("TX Queue Create Failed");
             return false;
@@ -586,7 +537,7 @@ static bool _init_bt(const char *deviceName, uint16_t rxQueueSize, uint16_t txQu
     }
 
     if(!_spp_task_handle){
-        xTaskCreatePinnedToCore(_spp_tx_task, "spp_tx", 4096, NULL, 10, &_spp_task_handle, 0);
+        xTaskCreatePinnedToCore(_spp_tx_task, "spp_tx", 4096, NULL, 2, &_spp_task_handle, 0);
         if(!_spp_task_handle){
             log_e("Network Event Task Start Failed!");
             return false;
@@ -696,10 +647,6 @@ static bool _stop_bt()
         vSemaphoreDelete(_spp_tx_done);
         _spp_tx_done = NULL;
     }
-    if (_bt_event_group) {
-        vEventGroupDelete(_bt_event_group);
-        _bt_event_group = NULL;
-    }
     return true;
 }
 
@@ -708,11 +655,6 @@ static bool waitForConnect(int timeout) {
     return (xEventGroupWaitBits(_spp_event_group, SPP_CONNECTED, pdFALSE, pdTRUE, xTicksToWait) & SPP_CONNECTED) != 0;
 }
 
-static bool waitForDiscovered(int timeout) {
-    TickType_t xTicksToWait = timeout / portTICK_PERIOD_MS;
-    return (xEventGroupWaitBits(_spp_event_group, BT_DISCOVERY_COMPLETED, pdFALSE, pdTRUE, xTicksToWait) & BT_DISCOVERY_COMPLETED) != 0;
-}
-
 /*
  * Serial Bluetooth Arduino
  *
@@ -857,11 +799,7 @@ bool BluetoothSerial::connect(String remoteName)
     _remote_name[ESP_BT_GAP_MAX_BDNAME_LEN] = 0;
     log_i("master : remoteName");
     // will first resolve name to address
-#ifdef ESP_IDF_VERSION_MAJOR
-        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
-#else
-        esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
-#endif
+    esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
     if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
         return waitForConnect(SCAN_TIMEOUT);
     }
@@ -901,11 +839,7 @@ bool BluetoothSerial::connect()
         disconnect();
         log_i("master : remoteName");
         // will resolve name to address first - it may take a while
-#ifdef ESP_IDF_VERSION_MAJOR
-        esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
-#else
         esp_bt_gap_set_scan_mode(ESP_BT_SCAN_MODE_CONNECTABLE_DISCOVERABLE);
-#endif
         if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, INQ_LEN, INQ_NUM_RSPS) == ESP_OK) {
             return waitForConnect(SCAN_TIMEOUT);
         }
@@ -952,75 +886,8 @@ bool BluetoothSerial::isReady(bool checkMaster, int timeout) {
     return (xEventGroupWaitBits(_spp_event_group, SPP_RUNNING, pdFALSE, pdTRUE, xTicksToWait) & SPP_RUNNING) != 0;
 }
 
-
-/**
- * @brief           RemoteName or address are not allowed to be set during discovery
- *                  (otherwhise it might connect automatically and stop discovery)
- * @param[in]       timeoutMs can range from MIN_INQ_TIME to MAX_INQ_TIME
- * @return          in case of Error immediately Empty ScanResults.
- */
-BTScanResults* BluetoothSerial::discover(int timeoutMs) {
-    scanResults.clear();
-    if (timeoutMs < MIN_INQ_TIME || timeoutMs > MAX_INQ_TIME || strlen(_remote_name) || _isRemoteAddressSet)
-        return nullptr;
-    int timeout = timeoutMs / INQ_TIME;
-    log_i("discover::disconnect");
-    disconnect();
-    log_i("discovering");
-    // will resolve name to address first - it may take a while
-    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
-    if (esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK) {
-        waitForDiscovered(timeoutMs);
-        esp_bt_gap_cancel_discovery();
-    }
-    return &scanResults;
-}
-
-/**
- * @brief           RemoteName or address are not allowed to be set during discovery
- *                  (otherwhise it might connect automatically and stop discovery)
- * @param[in]       cb called when a [b]new[/b] device has been discovered
- * @param[in]       timeoutMs can be 0 or range from MIN_INQ_TIME to MAX_INQ_TIME
- *
- * @return          Wheter start was successfull or problems with params
- */
-bool BluetoothSerial::discoverAsync(BTAdvertisedDeviceCb cb, int timeoutMs) {
-    scanResults.clear();
-    if (strlen(_remote_name) || _isRemoteAddressSet)
-        return false;
-    int timeout = timeoutMs / INQ_TIME;
-    disconnect();
-    advertisedDeviceCb = cb;
-    log_i("discovering");
-    // will resolve name to address first - it may take a while
-    esp_bt_gap_set_scan_mode(ESP_BT_CONNECTABLE, ESP_BT_GENERAL_DISCOVERABLE);
-    if (timeout > 0)
-        return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, timeout, 0) == ESP_OK;
-    else return esp_bt_gap_start_discovery(ESP_BT_INQ_MODE_GENERAL_INQUIRY, ESP_BT_GAP_MAX_INQ_LEN, 0) == ESP_OK;
+bool BluetoothSerial::isCongested(){
+    return(!(xEventGroupGetBits(_spp_event_group) & SPP_CONGESTED));
 }
 
-/** @brief      Stops the asynchronous discovery and clears the callback */
-void BluetoothSerial::discoverAsyncStop() {
-    esp_bt_gap_cancel_discovery();
-    advertisedDeviceCb = nullptr;
-}
-
-/** @brief      Clears scanresult entries */
-void BluetoothSerial::discoverClear() {
-    scanResults.clear();
-}
-
-/** @brief      Can be used while discovering asynchronously
- *              Will be returned also on synchronous discovery.
- *
- * @return      BTScanResults contains several information of found devices
- */
-BTScanResults* BluetoothSerial::getScanResults() {
-    return &scanResults;
-}
-
-BluetoothSerial::operator bool() const
-{
-    return true;
-}
 #endif
diff --git a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
index d7d52bd21..6c4d2d3e3 100644
--- a/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
+++ b/Firmware/RTK_Surveyor/src/BluetoothSerial/BluetoothSerial.h
@@ -21,15 +21,12 @@
 
 #include "Arduino.h"
 #include "Stream.h"
-#include <esp_gap_bt_api.h>
 #include <esp_spp_api.h>
 #include <functional>
-#include "BTScan.h"
 
 typedef std::function<void(const uint8_t *buffer, size_t size)> BluetoothSerialDataCb;
 typedef std::function<void(uint32_t num_val)> ConfirmRequestCb;
 typedef std::function<void(boolean success)> AuthCompleteCb;
-typedef std::function<void(BTAdvertisedDevice* pAdvertisedDevice)> BTAdvertisedDeviceCb;
 
 class BluetoothSerial: public Stream
 {
@@ -39,9 +36,6 @@ class BluetoothSerial: public Stream
         ~BluetoothSerial(void);
 
         bool begin(String localName=String(), bool isMaster=false, uint16_t rxQueueSize = 512 * 4, uint16_t txQueueSize = 512);
-        bool begin(unsigned long baud){//compatibility
-            return begin();
-        }
         int available(void);
         int peek(void);
         bool hasClient(void);
@@ -67,17 +61,8 @@ class BluetoothSerial: public Stream
         bool disconnect();
         bool unpairDevice(uint8_t remoteAddress[]);
 
-        BTScanResults* discover(int timeout=0x30*1280);
-        bool discoverAsync(BTAdvertisedDeviceCb cb, int timeout=0x30*1280);
-        void discoverAsyncStop();
-        void discoverClear();
-        BTScanResults* getScanResults();
-        
-        const int INQ_TIME = 1280;   // Inquire Time unit 1280 ms
-        const int MIN_INQ_TIME = (ESP_BT_GAP_MIN_INQ_LEN * INQ_TIME);
-        const int MAX_INQ_TIME = (ESP_BT_GAP_MAX_INQ_LEN * INQ_TIME);
-        
-        operator bool() const;
+        bool isCongested();
+
     private:
         String local_name;
 

From 818d53e79bd1db3c917b02c20b4516d43356f676 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 12:29:44 -0600
Subject: [PATCH 14/17] Fix entry into test menu

---
 Firmware/RTK_Surveyor/Display.ino      |  2 +-
 Firmware/RTK_Surveyor/RTK_Surveyor.ino |  3 +++
 Firmware/RTK_Surveyor/States.ino       | 14 +++++++++-----
 Firmware/RTK_Surveyor/Tasks.ino        |  1 +
 4 files changed, 14 insertions(+), 6 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Display.ino b/Firmware/RTK_Surveyor/Display.ino
index d11fadf01..857f7c234 100644
--- a/Firmware/RTK_Surveyor/Display.ino
+++ b/Firmware/RTK_Surveyor/Display.ino
@@ -86,7 +86,7 @@ void updateDisplay()
           displayWiFiConfig(); //Display SSID and IP
           break;
         case (STATE_TEST):
-          //Do nothing
+          paintSystemTest();
           break;
         case (STATE_TESTING):
           paintSystemTest();
diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index 61eeeec1d..4e303f634 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -6,6 +6,8 @@
   This firmware runs the core of the SparkFun RTK Surveyor product. It runs on an ESP32
   and communicates with the ZED-F9P.
 
+  Compiled with Arduino v1.8.13 with ESP32 core v1.0.6.
+
   Select the ESP32 Dev Module from the boards list. This maps the same pins to the ESP32-WROOM module.
   Select 'Minimal SPIFFS (1.9MB App)' from the partition list. This will enable SD firmware updates.
 
@@ -349,6 +351,7 @@ uint16_t svinObservationTime = 0; //Use globals so we don't have to request thes
 float svinMeanAccuracy = 0;
 
 uint32_t lastSetupMenuChange = 0;
+uint32_t lastTestMenuChange = 0; //Avoids exiting the test menu for at least 1 second
 //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
 
 void setup()
diff --git a/Firmware/RTK_Surveyor/States.ino b/Firmware/RTK_Surveyor/States.ino
index d10b04742..679a9425c 100644
--- a/Firmware/RTK_Surveyor/States.ino
+++ b/Firmware/RTK_Surveyor/States.ino
@@ -636,13 +636,17 @@ void updateSystemState()
       //Setup device for testing
       case (STATE_TEST):
         {
-          //Enable RTCM 1230. This is the GLONASS bias sentence and is transmitted
-          //even if there is no GPS fix. We use it to test serial output.
-          i2cGNSS.enableRTCMmessage(UBX_RTCM_1230, COM_PORT_UART2, 1); //Enable message every second
+          //Don't enter testing
+          if (millis() - lastTestMenuChange > 500)
+          {
+            //Enable RTCM 1230. This is the GLONASS bias sentence and is transmitted
+            //even if there is no GPS fix. We use it to test serial output.
+            i2cGNSS.enableRTCMmessage(UBX_RTCM_1230, COM_PORT_UART2, 1); //Enable message every second
 
-          inTestMode = true; //Reroutes bluetooth bytes
+            inTestMode = true; //Reroutes bluetooth bytes
 
-          changeState(STATE_TESTING);
+            changeState(STATE_TESTING);
+          }
         }
         break;
 
diff --git a/Firmware/RTK_Surveyor/Tasks.ino b/Firmware/RTK_Surveyor/Tasks.ino
index b2d7049ac..7fa025bfa 100644
--- a/Firmware/RTK_Surveyor/Tasks.ino
+++ b/Firmware/RTK_Surveyor/Tasks.ino
@@ -219,6 +219,7 @@ void ButtonCheckTask(void *e)
       {
         forceSystemStateUpdate = true;
         requestChangeState(STATE_TEST);
+        lastTestMenuChange = millis(); //Avoid exiting test menu for 1s
       }
       else if (setupBtn != NULL && setupBtn->wasReleased())
       {

From 88aff51e946766cbb4e1130df4a72fa57d45babc Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 12:30:05 -0600
Subject: [PATCH 15/17] Fix entry into test menu.

---
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index 4e303f634..a4dc32694 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -223,7 +223,7 @@ const uint8_t F9PSerialWriteTaskPriority = 1; //3 being the highest, and 0 being
 const uint8_t F9PSerialReadTaskPriority = 1;
 
 TaskHandle_t pinUART2TaskHandle = NULL; //Dummy task to start UART2 on core 0.
-bool uart2pinned = false;
+volatile bool uart2pinned = false; //This variable is touched by core 0 but checked by core 1. Must be volatile.
 
 //Reduced stack size from 10,000 to 2,000 to make room for WiFi/NTRIP server capabilities
 const int readTaskStackSize = 2500;
@@ -350,7 +350,7 @@ bool setupByPowerButton = false; //We can change setup via tapping power button
 uint16_t svinObservationTime = 0; //Use globals so we don't have to request these values multiple times (slow response)
 float svinMeanAccuracy = 0;
 
-uint32_t lastSetupMenuChange = 0;
+uint32_t lastSetupMenuChange = 0; //Auto-selects the setup menu option after 1500ms
 uint32_t lastTestMenuChange = 0; //Avoids exiting the test menu for at least 1 second
 //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-
 
@@ -486,7 +486,6 @@ void updateLogs()
       }
     }
   }
-  //  }
 }
 
 //Once we have a fix, sync system clock to GNSS

From 7c619991028b5a7192266c861a8c137e23081915 Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 13:03:09 -0600
Subject: [PATCH 16/17] Enable logging during base. Trim display for reset
 count.

---
 Firmware/RTK_Surveyor/Base.ino    | 22 +++++++++++--------
 Firmware/RTK_Surveyor/Display.ino | 36 +++++++++++++++++++++++++------
 Firmware/RTK_Surveyor/States.ino  |  1 +
 3 files changed, 44 insertions(+), 15 deletions(-)

diff --git a/Firmware/RTK_Surveyor/Base.ino b/Firmware/RTK_Surveyor/Base.ino
index b423166c0..8b185631c 100644
--- a/Firmware/RTK_Surveyor/Base.ino
+++ b/Firmware/RTK_Surveyor/Base.ino
@@ -41,7 +41,7 @@ bool configureUbloxModuleBase()
   getPortSettings(COM_PORT_I2C); //Load the settingPayload with this port's settings
   if (settingPayload[OUTPUT_SETTING] != (COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3))
     response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_NMEA | COM_TYPE_RTCM3); //Set the I2C port to output UBX (config), and RTCM3 (casting)
-  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+NMEA+RTCM3 - 
+  //response &= i2cGNSS.setPortOutput(COM_PORT_I2C, COM_TYPE_UBX | COM_TYPE_RTCM3); //Not a valid state. Goes to UBX+NMEA+RTCM3 -
 
   //In base mode the Surveyor should output RTCM over UART2 and I2C ports:
   //(Primary) UART2 in case the Surveyor is connected via radio to rover
@@ -99,10 +99,10 @@ bool beginSurveyIn()
   //Wait until active becomes true
   long maxTime = 5000;
   long startTime = millis();
-  while(i2cGNSS.getSurveyInActive(100) == false)
+  while (i2cGNSS.getSurveyInActive(100) == false)
   {
     delay(100);
-    if(millis() - startTime > maxTime) return(false); //Reset of survey failed
+    if (millis() - startTime > maxTime) return (false); //Reset of survey failed
   }
 
   return (true);
@@ -119,19 +119,19 @@ bool resetSurvey()
   delay(1000);
   response &= i2cGNSS.disableSurveyMode(maxWait); //Disable survey
 
-  if(response == false)
-    return(response);
+  if (response == false)
+    return (response);
 
   //Wait until active and valid becomes false
   long maxTime = 5000;
   long startTime = millis();
-  while(i2cGNSS.getSurveyInActive(100) == true || i2cGNSS.getSurveyInValid(100) == true)
+  while (i2cGNSS.getSurveyInActive(100) == true || i2cGNSS.getSurveyInValid(100) == true)
   {
     delay(100);
-    if(millis() - startTime > maxTime) return(false); //Reset of survey failed
+    if (millis() - startTime > maxTime) return (false); //Reset of survey failed
   }
 
-  return(true);
+  return (true);
 }
 
 //Start the base using fixed coordinates
@@ -212,7 +212,11 @@ void SFE_UBLOX_GNSS::processRTCM(uint8_t incoming)
   }
 
   //Check for too many digits
-  if (logIncreasing == true)
+  if (settings.enableResetDisplay == true)
+  {
+    if (rtcmPacketsSent > 99) rtcmPacketsSent = 1; //Trim to two digits to avoid overlap
+  }
+  else if (logIncreasing == true)
   {
     if (rtcmPacketsSent > 999) rtcmPacketsSent = 1; //Trim to three digits to avoid log icon
   }
diff --git a/Firmware/RTK_Surveyor/Display.ino b/Firmware/RTK_Surveyor/Display.ino
index 857f7c234..63d7fba4e 100644
--- a/Firmware/RTK_Surveyor/Display.ino
+++ b/Firmware/RTK_Surveyor/Display.ino
@@ -445,12 +445,7 @@ void paintSIV()
       oled.print(i2cGNSS.getSIV());
     }
 
-    if (settings.enableResetDisplay == true)
-    {
-      oled.setFontType(0); //Small font
-      oled.setCursor(16 + (8 * 3) + 6, 38); //x, y
-      oled.print(settings.resetCount);
-    }
+    paintResets();
   }
 }
 
@@ -646,6 +641,8 @@ void paintBaseTempTransmitting()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -680,6 +677,8 @@ void paintBaseTempWiFiStarted()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -715,6 +714,8 @@ void paintBaseTempWiFiConnected()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -776,6 +777,8 @@ void paintBaseTempCasterConnected()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -822,6 +825,8 @@ void paintBaseFixedTransmitting()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -856,6 +861,8 @@ void paintBaseFixedWiFiStarted()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -891,6 +898,8 @@ void paintBaseFixedWiFiConnected()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -952,6 +961,8 @@ void paintBaseFixedCasterConnected()
     oled.setFontType(1); //Set font to type 1: 8x16
     oled.print(rtcmPacketsSent); //rtcmPacketsSent is controlled in processRTCM()
 
+    paintResets();
+
     paintLogging();
   }
 }
@@ -1558,3 +1569,16 @@ void displayMessage(const char* message, uint16_t displayTime)
     delay(displayTime);
   }
 }
+
+void paintResets()
+{
+  if (online.display == true)
+  {
+    if (settings.enableResetDisplay == true)
+    {
+      oled.setFontType(0); //Small font
+      oled.setCursor(16 + (8 * 3) + 6, 38); //x, y
+      oled.print(settings.resetCount);
+    }
+  }
+}
diff --git a/Firmware/RTK_Surveyor/States.ino b/Firmware/RTK_Surveyor/States.ino
index 679a9425c..cd51198b6 100644
--- a/Firmware/RTK_Surveyor/States.ino
+++ b/Firmware/RTK_Surveyor/States.ino
@@ -124,6 +124,7 @@ void updateSystemState()
           //Stop all WiFi and BT. Re-enable in each specific base start state.
           stopWiFi();
           stopBluetooth();
+          startUART2Tasks(); //Start monitoring the UART1 from ZED for NMEA and UBX data (enables logging)
 
           if (configureUbloxModuleBase() == true)
           {

From 71f0845f5a6cc42f3c213ab4664e650266c638ab Mon Sep 17 00:00:00 2001
From: Nathan Seidle <nathan@sparkfun.com>
Date: Mon, 20 Sep 2021 13:43:13 -0600
Subject: [PATCH 17/17] Update RTK_Surveyor.ino

---
 Firmware/RTK_Surveyor/RTK_Surveyor.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Firmware/RTK_Surveyor/RTK_Surveyor.ino b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
index a4dc32694..f11cfab63 100644
--- a/Firmware/RTK_Surveyor/RTK_Surveyor.ino
+++ b/Firmware/RTK_Surveyor/RTK_Surveyor.ino
@@ -45,7 +45,7 @@ const int FIRMWARE_VERSION_MINOR = 6;
 
 #define COMPILE_WIFI //Comment out to remove all WiFi functionality
 #define COMPILE_BT //Comment out to disable all Bluetooth
-#define ENABLE_DEVELOPER //Uncomment this line to enable special developer modes (don't check power button at startup)
+//#define ENABLE_DEVELOPER //Uncomment this line to enable special developer modes (don't check power button at startup)
 
 //Define the RTK board identifier:
 //  This is an int which is unique to this variant of the RTK Surveyor hardware which allows us