From 45b3d6c1ec2179c3b1b21cdebebb041829cbd371 Mon Sep 17 00:00:00 2001
From: Paul <5690545+PaulZC@users.noreply.github.com>
Date: Mon, 18 May 2020 08:11:49 +0100
Subject: [PATCH] Correcting svin.meanAccuracy

---
 src/SparkFun_Ublox_Arduino_Library.cpp | 29 ++++++++++++++++++++------
 1 file changed, 23 insertions(+), 6 deletions(-)

diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp
index 6bcc68d..24c2ba3 100644
--- a/src/SparkFun_Ublox_Arduino_Library.cpp
+++ b/src/SparkFun_Ublox_Arduino_Library.cpp
@@ -2005,15 +2005,20 @@ boolean SFE_UBLOX_GPS::setSurveyMode(uint8_t mode, uint16_t observationTime, flo
     packetCfg.payload[x] = 0;
 
   //payloadCfg should be loaded with poll response. Now modify only the bits we care about
-  payloadCfg[2] = mode; //Set mode. Survey-In and Disabled are most common.
+  payloadCfg[2] = mode; //Set mode. Survey-In and Disabled are most common. Use ECEF (not LAT/LON/ALT). 
 
+  //svinMinDur is U4 (uint32_t) but we'll only use a uint16_t (waiting more than 65535 seconds seems excessive!)
   payloadCfg[24] = observationTime & 0xFF; //svinMinDur in seconds
   payloadCfg[25] = observationTime >> 8;   //svinMinDur in seconds
+  payloadCfg[26] = 0;   //Truncate to 16 bits
+  payloadCfg[27] = 0;   //Truncate to 16 bits
 
-  uint32_t svinAccLimit = requiredAccuracy * 10000; //Convert m to 0.1mm
+  //svinAccLimit is U4 (uint32_t) in 0.1mm.
+  uint32_t svinAccLimit = (uint32_t)(requiredAccuracy * 10000.0); //Convert m to 0.1mm
   payloadCfg[28] = svinAccLimit & 0xFF;             //svinAccLimit in 0.1mm increments
   payloadCfg[29] = svinAccLimit >> 8;
   payloadCfg[30] = svinAccLimit >> 16;
+  payloadCfg[31] = svinAccLimit >> 24;
 
   return ((sendCommand(&packetCfg, maxWait)) == SFE_UBLOX_STATUS_DATA_SENT); // We are only expecting an ACK
 }
@@ -2050,13 +2055,25 @@ boolean SFE_UBLOX_GPS::getSurveyStatus(uint16_t maxWait)
     return (false);                                                         //If command send fails then bail
 
   //We got a response, now parse the bits into the svin structure
-  svin.observationTime = extractLong(8);
 
+  //dur (Passed survey-in observation time) is U4 (uint32_t) seconds. We truncate to 16 bits
+  //(waiting more than 65535 seconds (18.2 hours) seems excessive!)
+  uint32_t tmpObsTime = extractLong(8);
+  if (tmpObsTime <= 0xFFFF)
+  {
+    svin.observationTime = (uint16_t)tmpObsTime;
+  }
+  else
+  {
+    svin.observationTime = 0xFFFF;
+  }
+
+  // meanAcc is U4 (uint32_t) in 0.1mm. We convert this to float.
   uint32_t tempFloat = extractLong(28);
-  svin.meanAccuracy = tempFloat / 10000.0; //Convert 0.1mm to m
+  svin.meanAccuracy = ((float)tempFloat) / 10000.0; //Convert 0.1mm to m
 
-  svin.valid = payloadCfg[36];
-  svin.active = payloadCfg[37]; //1 if survey in progress, 0 otherwise
+  svin.valid = payloadCfg[36]; //1 if survey-in position is valid, 0 otherwise
+  svin.active = payloadCfg[37]; //1 if survey-in in progress, 0 otherwise
 
   return (true);
 }