From 30620cbab8beb332c60016eb676ff8958c974f7b Mon Sep 17 00:00:00 2001
From: Paul <5690545+PaulZC@users.noreply.github.com>
Date: Wed, 22 Apr 2020 10:02:27 +0100
Subject: [PATCH] Added getDynamicModel

---
 .../Example19_DynamicModel.ino                | 27 ++++++++++++++-----
 keywords.txt                                  |  1 +
 src/SparkFun_Ublox_Arduino_Library.cpp        | 16 +++++++++++
 src/SparkFun_Ublox_Arduino_Library.h          |  1 +
 4 files changed, 38 insertions(+), 7 deletions(-)

diff --git a/examples/Example19_DynamicModel/Example19_DynamicModel.ino b/examples/Example19_DynamicModel/Example19_DynamicModel.ino
index 980c2b6..de8a1df 100644
--- a/examples/Example19_DynamicModel/Example19_DynamicModel.ino
+++ b/examples/Example19_DynamicModel/Example19_DynamicModel.ino
@@ -1,8 +1,8 @@
 /*
   Set Dynamic Model
   By: Paul Clark (PaulZC)
-  Date: March 9th, 2020
-  
+  Date: April 22nd, 2020
+
   Based extensively on Example3_GetPosition
   By: Nathan Seidle
   SparkFun Electronics
@@ -18,7 +18,7 @@
   SEA, AIRBORNE1g, AIRBORNE2g, AIRBORNE4g, WRIST, BIKE
 
   Note: Long/lat are large numbers because they are * 10^7. To convert lat/long
-  to something google maps understands simply divide the numbers by 10,000,000. We 
+  to something google maps understands simply divide the numbers by 10,000,000. We
   do this so that we don't have to use floating point numbers.
 
   Leave NMEA parsing behind. Now you can simply ask the module for the datums you want!
@@ -47,10 +47,12 @@ void setup()
   Serial.begin(115200);
   while (!Serial)
     ; //Wait for user to open terminal
-  Serial.println("SparkFun Ublox Example");
+  Serial.println(F("SparkFun Ublox Example"));
 
   Wire.begin();
 
+  //myGPS.enableDebugging(); // Uncomment this line to enable debug messages
+
   if (myGPS.begin() == false) //Connect to the Ublox module using Wire port
   {
     Serial.println(F("Ublox GPS not detected at default I2C address. Please check wiring. Freezing."));
@@ -58,7 +60,6 @@ void setup()
       ;
   }
 
-  //myGPS.enableDebugging(); // Uncomment this line to enable debug messages
   myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
 
   // If we are going to change the dynamic platform model, let's do it here.
@@ -67,11 +68,23 @@ void setup()
 
   if (myGPS.setDynamicModel(DYN_MODEL_PORTABLE) == false) // Set the dynamic model to PORTABLE
   {
-    Serial.println("***!!! Warning: setDynamicModel failed !!!***");
+    Serial.println(F("***!!! Warning: setDynamicModel failed !!!***"));
+  }
+  else
+  {
+    Serial.println(F("Dynamic platform model changed successfully!"));
+  }
+
+  // Let's read the new dynamic model to see if it worked
+  uint8_t newDynamicModel = myGPS.getDynamicModel();
+  if (newDynamicModel == 255)
+  {
+    Serial.println(F("***!!! Warning: getDynamicModel failed !!!***"));
   }
   else
   {
-    Serial.println("Dynamic platform model changed successfully!");
+    Serial.print(F("The new dynamic model is: "));
+    Serial.println(newDynamicModel);
   }
 
   //myGPS.saveConfigSelective(VAL_CFG_SUBSEC_NAVCONF); //Uncomment this line to save only the NAV settings to flash and BBR
diff --git a/keywords.txt b/keywords.txt
index 3bdbcf1..03b844c 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -134,6 +134,7 @@ clearGeofences	KEYWORD2
 getGeofenceState	KEYWORD2
 
 setDynamicModel	KEYWORD2
+getDynamicModel	KEYWORD2
 powerSaveMode	KEYWORD2
 
 configureMessage	KEYWORD2
diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp
index bc35672..ade8796 100644
--- a/src/SparkFun_Ublox_Arduino_Library.cpp
+++ b/src/SparkFun_Ublox_Arduino_Library.cpp
@@ -2537,6 +2537,22 @@ boolean SFE_UBLOX_GPS::setDynamicModel(dynModel newDynamicModel, uint16_t maxWai
   return (sendCommand(&packetCfg, maxWait) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
 
+//Get the dynamic platform model using UBX-CFG-NAV5
+//Returns 255 if the sendCommand fails
+uint8_t SFE_UBLOX_GPS::getDynamicModel(uint16_t maxWait)
+{
+  packetCfg.cls = UBX_CLASS_CFG;
+  packetCfg.id = UBX_CFG_NAV5;
+  packetCfg.len = 0;
+  packetCfg.startingSpot = 0;
+
+  //Ask module for the current navigation model settings. Loads into payloadCfg.
+  if (sendCommand(&packetCfg, maxWait) != SFE_UBLOX_STATUS_DATA_RECEIVED) // We are expecting data and an ACK
+    return (255);
+
+  return (payloadCfg[2]); // Return the dynamic model
+}
+
 //Given a spot in the payload array, extract four bytes and build a long
 uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart)
 {
diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h
index 7c517df..640d564 100644
--- a/src/SparkFun_Ublox_Arduino_Library.h
+++ b/src/SparkFun_Ublox_Arduino_Library.h
@@ -617,6 +617,7 @@ class SFE_UBLOX_GPS
 
 	//Change the dynamic platform model using UBX-CFG-NAV5
 	boolean setDynamicModel(dynModel newDynamicModel = DYN_MODEL_PORTABLE, uint16_t maxWait = 1100);
+	uint8_t getDynamicModel(uint16_t maxWait = 1100); // Get the dynamic model - returns 255 if the sendCommand fails
 
 	//Survey-in specific controls
 	struct svinStructure