Added functionality for reading from the High Precision Geodetic Position packet

coelacant2 · coelacant2 · commit 63fb62ebd12c · 2019-06-11T18:47:54.000-04:00
- Added example for printing the high precision latitude, longitude, and accuracy
- Added example for printing the entire HPPOSLLH packet
- Added functionality for reading HPPOSLLH (High Precision Geodetic Position)
- Added getTimeOfWeek(), getHighResLatitude(). getHighResLongitude(), getElipsoid(), getMeanSeaLevel(), getHorizontalAccuracy(), getVerticalAccuracy(), getHPPOSLLH()
- Modified ProcessUBXPacket to parse HPPOSLLH packet
- Added query staleness verification for HPPOSLLH data
diff --git a/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino b/examples/ZED-F9P/Example8_GetHighPrecisionPositionAndAccuracy/Example8_GetHighPrecisionPositionAndAccuracy.ino
@@ -0,0 +1,77 @@
+/*
+  Get the high precision geodetic solution for latituude and longitude
+  By: Nathan Seidle
+  Modified by: Steven Rowland
+  SparkFun Electronics
+  Date: January 3rd, 2019
+  License: MIT. See license file for more information but you can
+  basically do whatever you want with this code.
+
+  This example shows how to inspect the accuracy of the high-precision
+  positional solution.
+
+  Feel like supporting open source hardware?
+  Buy a board from SparkFun!
+  ZED-F9P RTK2: https://www.sparkfun.com/products/15136
+  NEO-M8P RTK: https://www.sparkfun.com/products/15005
+  SAM-M8Q: https://www.sparkfun.com/products/15106
+
+  Hardware Connections:
+  Plug a Qwiic cable into the GPS and a BlackBoard
+  If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
+  Open the serial monitor at 115200 baud to see the output
+*/
+
+#include <Wire.h> //Needed for I2C to GPS
+
+#include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS
+SFE_UBLOX_GPS myGPS;
+
+long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to Ublox module.
+
+void setup()
+{
+  Serial.begin(115200);
+  while (!Serial); //Wait for user to open terminal
+
+  Wire.begin();
+
+  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."));
+    while (1);
+  }
+
+  //myGPS.enableDebugging(Serial);
+
+  myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
+  myGPS.setNavigationFrequency(20); //Set output to 20 times a second
+
+  byte rate = myGPS.getNavigationFrequency(); //Get the update rate of this module
+  Serial.print("Current update rate:");
+  Serial.println(rate);
+  
+  myGPS.saveConfiguration(); //Save the current settings to flash and BBR
+}
+
+void loop()
+{
+  //Query module only every second. Doing it more often will just cause I2C traffic.
+  //The module only responds when a new position is available
+  if (millis() - lastTime > 1000)
+  {
+    lastTime = millis(); //Update the timer
+    Serial.print("HP Lat: ");
+    int32_t latitude = myGPS.getHighResLatitude();
+    Serial.print(latitude);
+    Serial.print(", HP Lon: ");
+
+    int32_t longitude = myGPS.getHighResLongitude();
+    Serial.print(longitude);
+    Serial.print(", Accuracy: ");
+
+    uint32_t accuracy = myGPS.getHorizontalAccuracy();
+    Serial.println(accuracy);
+  }
+
+}
diff --git a/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino b/examples/ZED-F9P/Example9_GetHighPrecisionGeodeticPacket/Example9_GetHighPrecisionGeodeticPacket.ino
@@ -0,0 +1,99 @@
+/*
+  Get the high precision geodetic solution
+  By: Nathan Seidle
+  Modified by: Steven Rowland
+  SparkFun Electronics
+  Date: January 3rd, 2019
+  License: MIT. See license file for more information but you can
+  basically do whatever you want with this code.
+
+  This example shows how to inspect the accuracy of the high-precision
+  positional solution.
+
+  Feel like supporting open source hardware?
+  Buy a board from SparkFun!
+  ZED-F9P RTK2: https://www.sparkfun.com/products/15136
+  NEO-M8P RTK: https://www.sparkfun.com/products/15005
+  SAM-M8Q: https://www.sparkfun.com/products/15106
+
+  Hardware Connections:
+  Plug a Qwiic cable into the GPS and a BlackBoard
+  If you don't have a platform with a Qwiic connection use the SparkFun Qwiic Breadboard Jumper (https://www.sparkfun.com/products/14425)
+  Open the serial monitor at 115200 baud to see the output
+*/
+
+#include <Wire.h> //Needed for I2C to GPS
+
+#include "SparkFun_Ublox_Arduino_Library.h" //http://librarymanager/All#SparkFun_Ublox_GPS
+SFE_UBLOX_GPS myGPS;
+
+long lastTime = 0; //Simple local timer. Limits amount if I2C traffic to Ublox module.
+
+void setup()
+{
+  Serial.begin(115200);
+  while (!Serial); //Wait for user to open terminal
+
+  Wire.begin();
+
+  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."));
+    while (1);
+  }
+
+  //myGPS.enableDebugging(Serial);
+
+  myGPS.setI2COutput(COM_TYPE_UBX); //Set the I2C port to output UBX only (turn off NMEA noise)
+  myGPS.setNavigationFrequency(20); //Set output to 20 times a second
+
+  byte rate = myGPS.getNavigationFrequency(); //Get the update rate of this module
+  Serial.print("Current update rate:");
+  Serial.println(rate);
+  
+  myGPS.saveConfiguration(); //Save the current settings to flash and BBR
+}
+
+void loop()
+{
+  //Query module only every second. Doing it more often will just cause I2C traffic.
+  //The module only responds when a new position is available
+  if (millis() - lastTime > 1000)
+  {
+    lastTime = millis(); //Update the timer
+    Serial.print("HP Lat: ");
+    int32_t latitude = myGPS.getHighResLatitude();
+    Serial.print(latitude);
+    Serial.print(", HP Lon: ");
+
+    int32_t longitude = myGPS.getHighResLongitude();
+    Serial.print(longitude);
+    Serial.print(", 2D Accuracy(MM): ");
+
+    uint32_t accuracy = myGPS.getHorizontalAccuracy();
+    Serial.println(accuracy);
+    Serial.print(", Vertical Accuracy(MM): ");
+
+    uint32_t verticalAccuracy = myGPS.getVerticalAccuracy();
+    Serial.println(verticalAccuracy);
+    Serial.print(", Elipsoid(MM): ");
+    
+    int32_t elipsoid = myGPS.getElipsoid();
+    Serial.println(elipsoid);
+    Serial.print(", Mean Sea Level(MM): ");
+    
+    int32_t meanSeaLevel = myGPS.getMeanSeaLevel();
+    Serial.println(meanSeaLevel);
+    Serial.print(", Geoid Separation(MM): ");
+    
+    int32_t geoidSeparation = myGPS.getGeoidSeparation();
+    Serial.println(geoidSeparation);
+    Serial.print(", Time of Week(Millis): ");
+    
+    uint32_t timeOfWeek = myGPS.getTimeOfWeek();
+    Serial.println(timeOfWeek);
+    Serial.print(",");
+    
+  }
+
+}
diff --git a/src/SparkFun_Ublox_Arduino_Library.cpp b/src/SparkFun_Ublox_Arduino_Library.cpp
@@ -17,6 +17,17 @@
   Development environment specifics:
   Arduino IDE 1.8.5
 
+  Modified by David Mann @ Loggerhead Instruments, 16 April 2019
+  - Added support for parsing date and time
+  - Added functions getYear(), getMonth(), getDay(), getHour(), getMinute(), getSecond()
+
+  Modified by Steven Rowland, June 11th, 2019
+  - Added functionality for reading HPPOSLLH (High Precision Geodetic Position)
+  - Added getTimeOfWeek(), getHighResLatitude(). getHighResLongitude(), getElipsoid(), 
+    getMeanSeaLevel(), getHorizontalAccuracy(), getVerticalAccuracy(), getHPPOSLLH()
+  - Modified ProcessUBXPacket to parse HPPOSLLH packet
+  - Added query staleness verification for HPPOSLLH data 
+
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
@@ -541,6 +552,38 @@ void SFE_UBLOX_GPS::processUBXpacket(ubxPacket *msg)
       moduleQueried.headingOfMotion = true;
       moduleQueried.pDOP = true;
     }
+    else if (msg->id == UBX_NAV_HPPOSLLH && msg->len == 36){
+      timeOfWeek = extractLong(4);
+      highResLatitude = extractLong(8);
+      highResLongitude = extractLong(12);
+      elipsoid = extractLong(16);
+      meanSeaLevel = extractLong(20);
+      geoidSeparation = extractLong(24);
+      horizontalAccuracy = extractLong(28);
+      verticalAccuracy = extractLong(32);
+      
+      highResModuleQueried.all = true;
+      highResModuleQueried.timeOfWeek = true;
+      highResModuleQueried.highResLatitude = true;
+      highResModuleQueried.highResLongitude = true;
+      highResModuleQueried.elipsoid = true;
+      highResModuleQueried.meanSeaLevel = true;
+      highResModuleQueried.geoidSeparation = true;
+      highResModuleQueried.horizontalAccuracy = true;
+      highResModuleQueried.verticalAccuracy = true;
+
+      if (_printDebug == true)
+      {
+        Serial.print("Sec: "); Serial.print(((float)extractLong(4)) / 1000.0f); Serial.print(" ");
+        Serial.print("LON: "); Serial.print(((float)(int32_t)extractLong(8)) / 10000000.0f); Serial.print(" ");
+        Serial.print("LAT: "); Serial.print(((float)(int32_t)extractLong(12)) / 10000000.0f); Serial.print(" ");
+        Serial.print("ELI M: "); Serial.print(((float)(int32_t)extractLong(16)) / 1000.0f); Serial.print(" ");
+        Serial.print("MSL M: "); Serial.print(((float)(int32_t)extractLong(20)) / 1000.0f); Serial.print(" ");
+        Serial.print("GEO: "); Serial.print(((float)(int32_t)extractLong(24))/ 1000.0f); Serial.print(" ");
+        Serial.print("HA 2D M: "); Serial.print(((float)extractLong(28)) / 1000.0f); Serial.print(" ");
+        Serial.print("VERT M: "); Serial.print(((float)extractLong(32)) / 1000.0f); Serial.print(" ");
+      }
+    }
     break;
   }
 }
@@ -721,8 +764,8 @@ void SFE_UBLOX_GPS::printPacket(ubxPacket *packet)
     _debugSerial->print(" ID:");
     _debugSerial->print(packet->id, HEX);
 
-    //_debugSerial->print(" Len: 0x");
-    //_debugSerial->print(packet->len, HEX);
+    _debugSerial->print(" Len: 0x");
+    _debugSerial->print(packet->len, HEX);
 
     _debugSerial->print(" Payload:");
 
@@ -1197,19 +1240,19 @@ boolean SFE_UBLOX_GPS::setAutoPVT(boolean enable, uint16_t maxWait)
 uint32_t SFE_UBLOX_GPS::extractLong(uint8_t spotToStart)
 {
   uint32_t val = 0;
-  val |= (int32_t)payloadCfg[spotToStart + 0] << 8 * 0;
-  val |= (int32_t)payloadCfg[spotToStart + 1] << 8 * 1;
-  val |= (int32_t)payloadCfg[spotToStart + 2] << 8 * 2;
-  val |= (int32_t)payloadCfg[spotToStart + 3] << 8 * 3;
+  val |= (uint32_t)payloadCfg[spotToStart + 0] << 8 * 0;
+  val |= (uint32_t)payloadCfg[spotToStart + 1] << 8 * 1;
+  val |= (uint32_t)payloadCfg[spotToStart + 2] << 8 * 2;
+  val |= (uint32_t)payloadCfg[spotToStart + 3] << 8 * 3;
   return (val);
 }
 
 //Given a spot in the payload array, extract two bytes and build an int
 uint16_t SFE_UBLOX_GPS::extractInt(uint8_t spotToStart)
 {
   uint16_t val = 0;
-  val |= (int16_t)payloadCfg[spotToStart + 0] << 8 * 0;
-  val |= (int16_t)payloadCfg[spotToStart + 1] << 8 * 1;
+  val |= (uint16_t)payloadCfg[spotToStart + 0] << 8 * 0;
+  val |= (uint16_t)payloadCfg[spotToStart + 1] << 8 * 1;
   return (val);
 }
 
@@ -1296,6 +1339,74 @@ boolean SFE_UBLOX_GPS::getPVT(uint16_t maxWait)
   }
 }
 
+uint32_t SFE_UBLOX_GPS::getTimeOfWeek(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.timeOfWeek == false)
+    getHPPOSLLH();
+  highResModuleQueried.timeOfWeek = false; //Since we are about to give this to user, mark this data as stale
+  return (timeOfWeek);
+}
+
+int32_t SFE_UBLOX_GPS::getHighResLatitude(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.highResLatitude == false)
+    getHPPOSLLH();
+  highResModuleQueried.highResLatitude = false; //Since we are about to give this to user, mark this data as stale
+  return (highResLatitude);
+}
+
+int32_t SFE_UBLOX_GPS::getHighResLongitude(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.highResLongitude == false)
+    getHPPOSLLH();
+  highResModuleQueried.highResLongitude = false; //Since we are about to give this to user, mark this data as stale
+  return (highResLongitude);
+}
+
+int32_t SFE_UBLOX_GPS::getElipsoid(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.elipsoid == false)
+    getHPPOSLLH();
+  highResModuleQueried.elipsoid = false; //Since we are about to give this to user, mark this data as stale
+  return (elipsoid);
+}
+
+int32_t SFE_UBLOX_GPS::getMeanSeaLevel(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.meanSeaLevel == false)
+    getHPPOSLLH();
+  highResModuleQueried.meanSeaLevel = false; //Since we are about to give this to user, mark this data as stale
+  return (meanSeaLevel);
+}
+
+int32_t SFE_UBLOX_GPS::getGeoidSeparation(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.geoidSeparation == false)
+    getHPPOSLLH();
+  highResModuleQueried.geoidSeparation = false; //Since we are about to give this to user, mark this data as stale
+  return (geoidSeparation);
+}
+
+uint32_t SFE_UBLOX_GPS::getHorizontalAccuracy(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.horizontalAccuracy == false)
+    getHPPOSLLH();
+  highResModuleQueried.horizontalAccuracy = false; //Since we are about to give this to user, mark this data as stale
+  return (horizontalAccuracy);
+}
+
+uint32_t SFE_UBLOX_GPS::getVerticalAccuracy(uint16_t maxWait/* = 250*/){
+  if (highResModuleQueried.verticalAccuracy == false)
+    getHPPOSLLH();
+  highResModuleQueried.verticalAccuracy = false; //Since we are about to give this to user, mark this data as stale
+  return (verticalAccuracy);
+}
+
+boolean SFE_UBLOX_GPS::getHPPOSLLH(uint16_t maxWait)
+{
+    //The GPS is not automatically reporting navigation position so we have to poll explicitly
+    packetCfg.cls = UBX_CLASS_NAV;
+    packetCfg.id = UBX_NAV_HPPOSLLH;
+    packetCfg.len = 0;
+
+    return sendCommand(packetCfg, maxWait);
+    return (false); //If command send fails then bail
+}
+
+
 //Get the current 3D high precision positional accuracy - a fun thing to watch
 //Returns a long representing the 3D accuracy in millimeters
 uint32_t SFE_UBLOX_GPS::getPositionAccuracy(uint16_t maxWait)
@@ -1557,4 +1668,4 @@ boolean SFE_UBLOX_GPS::getRELPOSNED(uint16_t maxWait)
   relPosInfo.refObsMiss = flags & (1 << 7);
 
   return (true);
-}
+}
diff --git a/src/SparkFun_Ublox_Arduino_Library.h b/src/SparkFun_Ublox_Arduino_Library.h
